Produce a yaml with new values of optimized parameters to be read by a configurable robot.urdf.xacro

[miguelriemoliveira]

From a discussion in #730, the idea of generating a yaml with the values of the calibrated parameters came up.
ATOM could produce this as it does an optimized urdf.

For reference:

On the side, please let me point out that I'm not at all a fan of outputting a calibrated URDF.
URDFs are generated from templates (mostly xacro in practice) and I don't want to fiddle with URDF diffs just because I want to regenerate my robot urdf with a new camera mounted somewhere. I just want to adapt the template and regenerate the urdf. So we use a yaml file for the calibration parameters and load that in xacro, adding the values in the correct spots.
My dream for an ideal calibration system for URDF would still be to define, e.g., in xacro ${1.57 + calibrate('name_of_dof')} as the value of anything, run calibration on the system and get back a yaml that defines the values of name_of_dof and use that automatically in the template. Just dreaming here :-)

[miguelriemoliveira]

Hi @v4hn and @manuelgitgomes ,

I just created a functionality in the calibrate script where it creates a yaml file called atom_calibration_params.yaml on the location of the dataset being used.

The names in calibration are derived from the calibration config.yml, e.g. for this configuration of a sensor in atom:

sensors:
    rgb_hand:
    link: "rgb_hand_optical_frame"
    parent_link: "flange"
    child_link: "rgb_hand_link"
    topic_name: "/rgb_hand/image_raw"
    throttle: 10
    modality: "rgb"

we get this is atom_calibration_params.yml

rgb_hand_pitch: -0.08898909991994362
rgb_hand_roll: -0.1099067372983195
rgb_hand_x: -0.06604520338605281
rgb_hand_y: -0.08283978769896988
rgb_hand_yaw: -0.1099067372983195
rgb_hand_z: 0.03310315346109225

and for the atom configuration of a joint, e.g.

joints:
  elbow_joint:
    params_to_calibrate: ['position_bias']

we get in the atom_calibration_params.yml:

elbow_joint_position_bias: 0.0

I think that this is what @v4hn was dreaming of. There is still some work to do with mapping the names used for the dof in the tams_pr2 xacro and the ones used in calibration, but that's a matter of matching the names used in the xacro with the ones defined in the atom configuration. However, not everything will be possible to match, i.e.:

For tams_pr2 calibration param:

head_pan_calib_ref: 0.0303234

we could have a configuration such as

joints:
  head_pan_calib:
    params_to_calibrate: ['position_bias']

which returns a calibrated params yaml like this:

head_pan_calib_position_bias: 8.4

I imagine you are not willing to change the name of the variables in https://github.com/TAMS-Group/tams_pr2/blob/noetic-devel/tams_pr2_description/config/calibration_config.yaml, so my suggestion is to develop a script that receives the atom_calibration_params.yml and a dictionary with the mapping of names and renames those yaml parameters as desired.

[v4hn]

Just to clarify, me dreaming involves that the only place I ever write down that a dof needs calibration is in the xacro and the ready-to-use yaml just drops out from running a "calibrate" script on it. Not entirely feasible, but a great step in the direction 👍

As long as the names are all local to tams repositories, I see no problem with changing them.

I think yaml-namespacing for xyzrpy combinations makes sense for easier use in macros.

Thank you Santa. 🐰

[miguelriemoliveira]

Thank you Santa.

Merry Christmas!

[miguelriemoliveira]

yaml-namespacing for xyzrpy combinations

you mean like this?

rgb_hand:
  x: -0.06604520338605281
  y: -0.08283978769896988
  z: 0.03310315346109225
  roll: -0.1099067372983195
  pitch: -0.08898909991994362
  yaw: -0.1099067372983195

[v4hn]

you mean like this?
```
rgb_hand:
  x: -0.06604520338605281
  y: -0.08283978769896988
  z: 0.03310315346109225
  roll: -0.1099067372983195
  pitch: -0.08898909991994362
  yaw: -0.1099067372983195
```

yes, exactly.

[v4hn]

Another issue I just discussed with Manuel, for which I can't seem to find documentation for in the other issues yet, is absolute values vs residuals.

It seems like your values here are currently absolute values for the transforms and relative(?) offsets for the calibration references.

We currently use the variables the other way around in our xacro:

• for joint offsets, l_shoulder_pan_calib_ref: 0.82713499 we treat them as absolute values in the PR2 and provide a default if the value is not set:

<xacro:unless value="${shoulder_pan_calib_ref}">
    <calibration  rising="${(reflect*M_PI/4)*cal_r_shoulder_pan_gearing+cal_r_shoulder_pan_flag}" />
</xacro:unless>
<xacro:if value="${shoulder_pan_calib_ref}">
    <calibration  rising="${shoulder_pan_calib_ref}" />
</xacro:if>

• for transforms azure_kinect_camera_base_pos_x: 0.0 we use them as residuals (as it's easy to measure/know the rough values.

    <origin xyz="${0.0+0.022+0.026+calib['azure_kinect_camera_base_pos_x']} ${0.0+calib['azure_kinect_camera_base_pos_y']} ${0.095 + 0.068 + 0.008 + calib['azure_kinect_camera_base_pos_z']}" rpy="${calib['azure_kinect_camera_base_rot_r']} ${calib['azure_kinect_camera_base_rot_p']} ${calib['azure_kinect_camera_base_rot_y']}" />

This should obviously be cleaned up to use the same mechanism, but I'm unsure what would establish the best standard in xacro.

Note that there is no easy way to measure the absolute calibration reference on the running system as the drivers already add the calibration value to each joint message. So you have to add whatever comes out of the calibration to the currently-used value (which you otherwise never looked at). I hope your rosbags include the robot_description that was active when the data was collected? 😁

Also, I believe it makes sense to give a "sane" default value in the xacro which is used if there is no calibration available, but the xacro:unless/if approach is very verbose. It would be great to support the residual notation we use in the azure, but it is also more cumbersome to extract the right values for the variables then.

Depending on what xacro allows (or what we get the xacro maintainer to support, he's open-minded after all 🤓) I could also imagine a standard statement like

<calibration  rising="${atom('shoulder_pan_calib_ref', (reflect*pi/4)*cal_r_shoulder_pan_gearing+cal_r_shoulder_pan_flag)}" />

for easier parsing on your end.

[manuelgitgomes]

relative(?) offsets for the calibration references.

Yes, for now the calibration references are summed to the ones seen in the URDF.

Depending on what xacro allows (or what we get the xacro maintainer to support, he's open-minded after all 🤓) I could also imagine a standard statement like

From a read of their wiki, it does not seem to be supported.
An old PR on the xacro repo suggested to add "arbitrary xml-generating python code", which could be one way to do this.
However, the discussion seemed to mostly move towards "xacro should not support this".

I see that Xacro already supports None types if they are alone (i.e., we cannot use operations, such as addition, on two different types, such as float and None).
Xacro could parse a robot description with Nones and ATOM would analyze that description and calibrate the parameters that are None.
The inclusion of defaults would be more tricky.
They could be defined in the same yaml file and ATOM could read it and republish the robot description, so we could run an uncalibrated system to gather data.
We can also discard Nones and make ATOM assume that if the value on the robot description is the default one, it calibrates it. This might not be the best solution, as it can create problems that I cannot envision right now.

[miguelriemoliveira]

Hi @v4hn and @manuelgitgomes ,

I went back to this. First, nesting parameters. Now we have the produced yaml like this:

sensors:
  rgb_world:
    x: -0.01625234687483334
    y: 0.007016927088536094
    z: 0.04058858883102345
    roll: -0.0035203431034870943
    pitch: -0.00850978852837561
    yaw: -0.01721668031464485
joints:
  elbow_joint:
    origin_x: -0.5996745659350029
    origin_y: -0.00048636814021923297
    origin_z: 0.009405942102471837
    origin_roll: -0.014553428819016492
    origin_pitch: -0.02173367222479105
    origin_yaw: 0.02605134752247605
    position_bias: -0.0190437204388629
  shoulder_lift_joint:
    origin_x: 0.016431914541455865
    origin_y: -0.015921078340292362
    origin_z: 0.010636931179058638
    origin_roll: 1.5415716586341253
    origin_pitch: 0.014863657572835746
    origin_yaw: -0.07258247459143602
    position_bias: 0.001961946752223525
  shoulder_pan_joint:
    origin_x: -0.0011072022674218484
    origin_y: 0.006104470330633574
    origin_z: 0.1776131205352634
    origin_roll: -0.024334800436751625
    origin_pitch: -0.028227058847477848
    origin_yaw: -0.06020146484737336
    position_bias: -0.06662659575563964

in the case of the riwmpbot. Check the calibration configuration here.

About the produced yaml file. My intent is to widen the usage of atom as much as possible. From that perspective I think adding to the yaml the keys sensors, additional_tfs (not there because this calibration had none) and joints makes sense. Actually, this way we have a direct mapping from the calibration config file to the produced yaml parameters as a result.

So I think this makes sense in general. However, I can understand that you prefer some tweak in the yaml for your particular system. The solution for that I think, if you really want a specific format, should be for us to produce a specific atom result exporter to tams format. That's easy to do since we just have to copy and adapt this function.

[miguelriemoliveira]

It seems like your values here are currently absolute values for the transforms and relative(?) offsets for the calibration references.

We are using absolute values for all parameters, i.e., transformations and joint origin xyz and rpy.
The only exception is the joint position bias, which is a relative value that should be added to the measured joint position.

We can only optimize constant parameters. This is the reason why we only estimate static transformations or, if they are dynamic (when a pattern is moving throughout the scene) we estimate one transform per collection.

In the case of the joint position bias, if we were to estimate instead the joint position, since its dynamic we would have to have one parameter per collection, which does not make sense. So from the perspective of the optimization it makes perfect sense to optimize the joint position bias.

Nonetheless, as before, I think we can fine tune a tams exporter for whatever you prefer. I think it should be @manuelgitgomes to work on this since he will be closer to the PR2.

From my side I will produce the "general" yaml format (as well as an urdf) which @manuelgitgomes can use as examples.

[miguelriemoliveira]

Closing this one as I think the tams exporter should be in a different issue.

[manuelgitgomes]

FYI, the tams exporter in now done in:

https://github.com/manuelgitgomes/tams_pr2/blob/noetic-devel/tams_pr2_atom_calibration/scripts/atom_dataset_to_tams_pr2_description

I opted to do it on the tams_pr2 repository because it would be only useful there.