SDFormat URDF to SDF converter silently set to identity sensor pose if sensor is attached to lumped link

[traversaro]

The SDFormat URDF to SDF converter if fixed joints are present in the URDF, fuses the child link with its parent, transferring all the information from the lumped link to the remaining link. However, it seems that due to a bug the sensor pose are not correctly propagated.

The issue is clear with this example (done with Gazebo 11.1). Let's say that we have two URDF files:

model_sensor_on_root_link.urdf :

<robot name="iCub">
  <link name="root_link">
    <inertial>
      <origin xyz="1.0 2.0 3.0 " rpy="0.1 0.2 0.3"/>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <gazebo reference="root_link">
    <sensor name="root_link_imu_acc" type="imu">
      <always_on>1</always_on>
      <update_rate>400</update_rate>
      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
      <plugin name="root_link_xsens_imu_plugin" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationFile>model://iCub/conf/gazebo_icub_xsens_inertial.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <link name="base_link"/>
  <joint name="base_fixed_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 -0 0"/>
    <axis xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="root_link"/>
  </joint>
</robot>

model_sensor_on_base_link.urdf :

<robot name="iCub">
  <link name="root_link">
    <inertial>
      <origin xyz="1.0 2.0 3.0 " rpy="0.1 0.2 0.3"/>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <gazebo reference="base_link">
    <sensor name="root_link_imu_acc" type="imu">
      <always_on>1</always_on>
      <update_rate>400</update_rate>
      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
      <plugin name="root_link_xsens_imu_plugin" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationFile>model://iCub/conf/gazebo_icub_xsens_inertial.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <link name="base_link"/>
  <joint name="base_fixed_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 -0 0"/>
    <axis xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="root_link"/>
  </joint>
</robot>

The only difference between the two files is the line <gazebo reference="root_link"> and <gazebo reference="base_link"> in another. The URDF files can be converted to SDF with the following command:

traversaro@IITICUBLAP102:~/icub-mdl-ws$ gz sdf -p model_sensor_on_root_link.urdf > model_sensor_on_root_link.sdf
traversaro@IITICUBLAP102:~/icub-mdl-ws$ gz sdf -p model_sensor_on_base_link.urdf > model_sensor_on_base_link.sdf

that results in the following output:
model_sensor_on_root_link.urdf :

<sdf version='1.7'>
  <model name='iCub'>
    <link name='base_link'>
      <inertial>
        <pose>1 2 3 0 -0 0</pose>
        <mass>1</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <sensor name='root_link_imu_acc' type='imu'>
        <always_on>1</always_on>
        <update_rate>400</update_rate>
        <plugin name='root_link_xsens_imu_plugin' filename='libgazebo_yarp_imu.so'>
          <yarpConfigurationFile>model://iCub/conf/gazebo_icub_xsens_inertial.ini</yarpConfigurationFile>
        </plugin>
        <pose>0 0 0 0 -0 0</pose>
      </sensor>
    </link>
  </model>
</sdf>

model_sensor_on_base_link.sdf :

<sdf version='1.7'>
  <model name='iCub'>
    <link name='base_link'>
      <inertial>
        <pose>1 2 3 0 -0 0</pose>
        <mass>1</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <sensor name='root_link_imu_acc' type='imu'>
        <always_on>1</always_on>
        <update_rate>400</update_rate>
        <pose>1 2 3 0.1 0.2 0.3</pose>
        <plugin name='root_link_xsens_imu_plugin' filename='libgazebo_yarp_imu.so'>
          <yarpConfigurationFile>model://iCub/conf/gazebo_icub_xsens_inertial.ini</yarpConfigurationFile>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

The difference between this two files:

traversaro@IITICUBLAP102:~/icub-mdl-ws$ diff model_sensor_on_root_link.sdf  model_sensor_on_base_link.sdf
20a21
>         <pose>1 2 3 -2.28319 -1.28319 -0.283185</pose>
24d24
<         <pose>0 0 0 0 -0 0</pose>

So it is possible to see that in the model_sensor_on_base_link.sdf case works fine, while in the case of model_sensor_on_root_link.sdf the pose of the sensor is silently set to the identity.

[traversaro]

In the model generated by this repo, this affects:

• Sensors root_link_imu_acc just for _plus models
• Sensors root_link_ems_acc_eb5 and root_link_ems_gyro_eb5 (see https://github.com/robotology/icub-models/blob/472db4555ea72832db571b49197849e1fc6192f1/iCub/robots/iCubGazeboV2_5_plus/model.urdf#L2968 and https://github.com/robotology/icub-models/blob/472db4555ea72832db571b49197849e1fc6192f1/iCub/robots/iCubGazeboV2_5_plus/model.urdf#L2187) for all iCub >= 2.5.5 models

[traversaro]

Getting a fix in sdformat properly deployed will take time. For a faster fix, the best option to be seems to just to avoid to have the peculiar structure for which the base_link is a parent link of the real link root_link, but rather have the base_link being a child of root_link, as all other fake massless links are child of the actual links.

The only reason we ever used the fake base_link as a parent was because it is a choice widespread in many humanoid robots, and because the kdl_parser used in ROS does not support the inertia for the root link, see:

• Add support for inertia in the root link  ros/kdl_parser#27
• KDL vs. Gazebo: Inertia in the root link ros/robot_model#6
• KDL-based algorithms fail for a tree without a "fake" base  idyntree#118

However, iDynTree has migrated away from using anything related to KDL since 2015, and KDL dynamics does not support floating base dynamics, so getting on the model generation side to basically workaround a decade old bug (ros/kdl_parser#27) is not a great tradeoff.

[traversaro]

We worked around the issue in #142, and the underlying issue (gazebosim/sdformat#378) was fixed in sdformat in gazebosim/sdformat#1114 . While the fix is not available in Gazebo 11 apt binary packages in Ubuntu 20.04/22.04 that many user use, I think that given that we worked around this issue in our models and that upstream is fixed, we can just close this issue.