search

robotology / icub-models-generator

Resources and programs to generated models (URDF, SDF) of the iCub robot
14 stars 23 forks source link

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

Closed traversaro closed 7 months ago

traversaro commented 3 years ago

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 commented 3 years ago

In the model generated by this repo, this affects:

traversaro commented 3 years ago

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:

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 (https://github.com/ros/kdl_parser/issues/27) is not a great tradeoff.

traversaro commented 7 months ago

We worked around the issue in https://github.com/robotology/icub-models-generator/pull/142, and the underlying issue (https://github.com/gazebosim/sdformat/issues/378) was fixed in sdformat in https://github.com/gazebosim/sdformat/pull/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.