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]

Upstream issue: robotology/icub-models-generator#140 .

[peci1]

I can see this behavior with gz sdf -p from Gazebo 9, but I don't see it with ign sdf -p from sdformat 10.3.

[peci1]

I've just published a related fix for ign sdf -p in SDFormat 10.3: #525 .

[peci1]

The fix from #525 has been released into libsdformat 10.4.0. But I'm not sure if it fixed this issue or not. Moreover, the issue was reported to Gazebo 11, which uses libsdformat 9.

[scpeters]

potential fix in #1114

[scpeters]

fixed by #1114

[traversaro]

Thanks @scpeters !

[scpeters]

Thanks @scpeters !

you're welcome! sorry it took so long 😅