Updating Kinematic Calibration

Summary

We've recently begun to develop several algorithms that allow us improve the calibration of the kinematic chains and sensor locations throughout the systems. Some examples of this are calibration flag offsets, link lengths/skews, and sensor placement. The tricky part is that the mechanism_model is not the only node is the system that uses the URDF. Motion planners, for example, will definitely also need an up-to-date robot model.

Here are a couple potential ideas for updating the calibration, and propagating the updates to other nodes.

Questions / Concerns / Comments

Current Proposal: Robot changeset file

Create a file that defines all the things that we want to change on the robot. This is more powerful than xacro, because we can theoretically do more high-level changes, as opposed to just string replacements.

When recalibrating the arm, the output of the calibration is an initial transform, defining the initial joint of a DH chain, and a set of DH parameters. This could potentially be encoded in the changeset as follows.

<chain_update root="torso_lift_link" tip="r_wrist_roll_link">
  <transform xyz="0, -.188, 0"
             rpy="0,     0, 0" />
  <dh_link name="r_arm_1" theta="0"    alpha="-pi/2" a=".1" d="0" />
  <dh_link name="r_arm_2" theta="pi/2" alpha=" pi/2" a="0"  d="0" />
  <dh_link name="r_arm_3" theta="0"    alpha=" pi/2" a="0"  d=".4" />
  <dh_link name="r_arm_4" theta="0"    alpha=" pi/2" a="0"  d="0" />
  <dh_link name="r_arm_5" theta="0"    alpha="-pi/2" a="0"  d=".32025" />
  <dh_link name="r_arm_6" theta="0"    alpha=" pi/2" a="0"  d="0" />
  <dh_link name="r_arm_7" theta="0"    alpha="0"     a="0"  d="0" />
</chain>

Updates to frames attached to links is coupled to the corresponding chain's definition. This is because the transform from the chain's tip to the sensor is dependent on how the tip of the chain is defined. Thus, we need to reference the parent in the redefined chain, as opposed to the original chain.

<frame_update name="r_forearm_cam_optical_frame">
  <parent name="r_arm_5" />
  <transform xyz=".12 0 0"
             rpy="0 0 0" />
</frame>

Old Ideas

Xacro

We currently use xacro to add calibration data into the URDF before it is loaded on the parameter server. Calibration offset variables are in all the link definitions, and a calibration file is included, defining all the constants.

snippet from pr2_defs/defs/arm_defs.xml

<joint name="${side}_upper_arm_roll_joint" type="revolute">
  <axis xyz="1 0 0" />
  <anchor xyz="0 0 0" />

  <limit min="${reflect*1.55-2.35}" max="${reflect*1.55+2.35}" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
         k_position="100" k_velocity="2"
         safety_length_min="0.15" safety_length_max="0.15" />
  <calibration reference_position="${(reflect*M_PI/2)*cal_r_upper_arm_roll_gearing + cal_r_upper_arm_roll_flag}" values="1.5 -1" />

  <joint_properties damping="0.1" />
  <map name="${side}_upper_arm_roll_joint_ode" flag="gazebo">
    <elem key="stopKd" value="1.0" />
    <elem key="stopKp" value="1000000.0" />
    <elem key="fudgeFactor" value="0.5" />
  </map>
</joint>

snippet from pr2_defs/calibration/prf_cal.xml

  <property name="cal_r_upper_arm_roll_flag" value="-0.008987" />
  <property name="cal_r_elbow_flex_flag"     value="-0.021100" />
  <property name="cal_r_forearm_roll_flag"   value="-0.032453" />

comments:
+ Very quick and dirty
+ There is a 'human readable' cal file that can easily be updated, or reverted, or zeroed out.
+ The robot description is available on param server as an XML file (is this a pro?)
- Updates occur without any knowledge about kinematics, rotations. We are only doing basic math operations. This puts more burden on whomever is computing the cal values.
- Works as an XML preprocessing step. It's unclear how to update calibration online.
- Makes the URDF even more confusing.

Modify the Robot Model from within a controller

Every controller running within mechanism control has access to the robot model. There could then be a kinematic_calibration_controller that directly changed parameters in this model.

comments:
+ We already have parsed the URDF into a kinematic model, so we can reason about transforms and other kinematic constructs while applying calibration changes.
- We have to regenerate xml from the robot model to update robot_description

Have a node own the robot model

Currently everyone that wants the robot model reads a giant XML string from the param server, and parses it themselves. Instead, each node that wants this info could make a service call to a RobotModelMaintainer, which could then return a message that fully describes the kinematic model of the robot.

comments:
+ XML parsing would occur only once
- Letting each node to do it's own XML parsing allows nodes to use custom information embedded within the URDF

Treat the [[pr2_controller_manager]] as another hardware driver

We could potentially think of the robot's mechanism as just another device plugged into ROS that happens to have a given 'configuration'. Just as the hokuyo driver has configurable scan setting, or the USB speaker driver might have configurable volume settings, pr2_controller_manager could have a configurable robot model. The controller manager could then be 'reconfigured' with a new robot model in the same way we decide to do so for drivers. These reconfiguring ideas were discussed in an API review for driver_base

Create a 2nd set of TF Frames

Since we're probably making very small changes to the kinematic model (+/- 1%), it's possible that it's complete overkill to try and update the robot model. Instead, we could simply publish a 2nd set of TF frames that represent the calibrated robot's kinematics. Controllers and planners wouldn't be able to easily interpret this data, but it could be used by all the sensory pipeline.

Minutes

• Provide a config file that has a set of overrrides. Not a string replacement.

  • Requires that the RobotModel can output a URDF file

  • This config file could live on the param server
• Would be nice to save the history of the calibration.
  • Should be able to do via subversion
  • Could be a directory of old files
• Wheel diameter calibration could follow the same update model
• Is the URDF just a really fancy dictionary?
  • Possible 2 step approach:

  • Figure what the inputs to the RobotModelUpdater look like.

  • Since we now have the data in a meaningful structure, we can decide how to propagate the changes.