Entering impedance control is unreliable

[calderpg]

Calls to validateForImpedanceMode(...) for both joint and cartesian motion regularly fail, even immediately after updating the load data.

[calderpg]

Function validateForImpedanceMode(...) calls ValidatorForImpedanceControl.validateCurrentLoadSettingLBR(LBR robot) or ValidatorForImpedanceControl.validateCurrentLoadSetting(PhysicalObject robot_with_tool)

From decompiled JAR, this is the implementation:

public final class ValidatorForImpedanceControl
{
  private static final Logger LOG = LoggerFactory.getLogger(ValidatorForImpedanceControl.class);
  private static final int RANKMIN = 6;
  private static final int VARIANCEMAX = 10;
  private static final int CARTEXTAUMAX = 60;
  private static final int AXISEXTTAUMAX = 10;
  private static final String ERR_STR_LBR_EXT_TORQUE_OBS = "] of LBRExtTorqueObs@";
  
  private static LBR searchLBRInPhysicalObject(PhysicalObject objectWithLBR)
  {
    LBR aRobot = (LBR)RoboticsAPIUtilities.searchRobot(objectWithLBR
      .getDefaultMotionFrame());
    return aRobot;
  }
  
  public static boolean validateCurrentLoadSettingLBR(LBR anLBR, StringBuilder errString)
  {
    SunriseLBR theLBR = (SunriseLBR)anLBR;
    SunriseController theControl = (SunriseController)theLBR
      .getController();
    
    SSR ssr = SSRFactory.requestDFEData(theLBR.getName(), 
      theLBR.getObserverName() + "@" + theLBR.getName(), new String[] { "Variance", 
      "rank", "axisTauExtMsr", "extForceMsr" });
    theControl.sendAsynchronousSSR(ssr);
    Message ssrAnswer = ssr.await();
    double[] variance = ssrAnswer.getParamDoubleArray(0);
    double[] rank = ssrAnswer.getParamDoubleArray(1);
    double[] axisTauExtMsr = ssrAnswer.getParamDoubleArray(2);
    double[] cartTau = ssrAnswer.getParamDoubleArray(3);
    
    boolean paramValid = true;
    for (int i = 0; i < rank.length; i++) {
      if (rank[i] < 6.0D)
      {
        errString.append("rank of " + theLBR.getName() + " is " + rank[i] + 
          " should be greater or equal than:" + 6 + "\n");
        paramValid = false;
      }
    }
    for (int i = 0; i < variance.length; i++) {
      if (variance[i] >= 10.0D)
      {
        errString.append("variance[" + i + "] of LBRExtTorqueObs@" + 
          theLBR.getName() + " is " + variance[i] + 
          " should be less than " + 10 + "\n");
        paramValid = false;
      }
    }
    for (int i = 0; i < axisTauExtMsr.length; i++) {
      if (axisTauExtMsr[i] > 10.0D)
      {
        errString.append("axisTauExtMsr[" + i + "] of LBRExtTorqueObs@" + 
          theLBR.getName() + " is " + axisTauExtMsr[i] + 
          " should be less than " + 10 + "\n");
        paramValid = false;
      }
    }
    for (int i = 0; i < cartTau.length; i++) {
      if (cartTau[i] > 60.0D)
      {
        errString.append("cartExtTau[" + i + "] of LBRExtTorqueObs@" + 
          theLBR.getName() + " is " + cartTau[i] + 
          " should be less than" + 60 + "\n");
        paramValid = false;
      }
    }
    if (!paramValid) {
      LOG.error("Err LoadValidation failed due to\n" + errString);
    }
    return paramValid;
  }
  
  public static boolean validateCurrentLoadSetting(PhysicalObject theLBRwithTool, StringBuilder errString)
  {
    LBR anLBR = searchLBRInPhysicalObject(theLBRwithTool);
    return validateCurrentLoadSettingLBR(anLBR, errString);
  }
}

[calderpg]

Both Joint and Cartesian impedance control can be reliably entered if the configuration of the arm is similar to an elbow-up config if the robot was mounted flat on a table. In testing so far, impedance control can be reliably entered when the robot is mounted to a horizontal surface.

[dmcconachie]

I tried an elbow up configuration, and was not able to enter joint impedance mode. This is something that will need to be worked on.

[dmcconachie]

The left arm has no difficulty switching between control modes (at least when in position that
the GravityCompensation demo leaves it it.

[dmcconachie]

The configuration of the right arm was incorrect which is why it was entirely unable to do anything with impedance mode; when we mounted the arm back on the torso, the orientation of the arm was not changed back to the torso values.

[dmcconachie]

Based on some testing using impedance mode, this is still a problem when mounted on the torso. It is possible to enter impedance mode, however it has to be done in a configuration where the entire wrench can be measured effectively (i.e. not near a singularity).

To address this, we should see if we can avoid recreating the entire SmartServo[Lin] motion if possible and simply change the parameters when we are sent a new set of control mode parameters.

[bsaund]

#60 changes the Java code so that the smart servo is not recreated when switching from Joint Impedance mode to Joint Impedance mode, so no revalidation is required.

Some more details:
We can change the impedance and damping parameters without stopping and revalidating. We must at least stop, and I believe remake the smartservo when changing the other parameters.

I think the best option moving forward:
Embrace Impedance mode! If we want something like Joint Position mode, we could just use really high impedance parameters.
We need better controllers though for trajectory following.