simmechanics-to-urdf bug when exporting frames with the same link name

martinaxgloria commented 4 months ago

Working on:

https://github.com/robotology/icub-models-generator/issues/271

@Nicogene and I found a bug in simmechanics-to-urdf. We used iCubGazeboV2_7 model (icub-models on the latest devel) on Gazebo with ros2 nws since we wanted to visualize the model on RViz. Using the RViz plugin that allows to visualize the wrenches reading from the topic, we couldn't visualize anything and a bench of this error appeared in the RViz terminal:

Message Filter dropping message: frame 'l_arm_ft' at time 59.645 for reason 'discarding message because the queue is full'

Looking at the latest version of the URDF, I noticed that the frame for both l_arm_ft and r_arm_ft was missing, there was only the Gazebo tag. Looking into the YAML file used for generating the URDF, we realized that when we added the exportation of the IMUs for the FTs:

which have the same name as the FT, the exported frame gets overwritten (in the YAML, the IMU export comes after the FT sensor export, so the IMU link is present in the URDF).

We patched it by exporting the FT frames of the arms as XML blobs:

- |
       <link name="l_arm_ft"/>
  - |      
       <joint name="l_arm_ft_fixed_joint" type="fixed">
         <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
         <parent link="l_shoulder_3"/>
         <child link="l_arm_ft"/>
         <dynamics damping="0.1"/>
       </joint>
  - |
       <link name="r_arm_ft"/>
  - |   
       <joint name="r_arm_ft_fixed_joint" type="fixed">
         <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
         <parent link="r_shoulder_3"/>
         <child link="r_arm_ft"/>
         <dynamics damping="0.1"/>
       </joint>
  - |

This worked. For the time being, I could open a PR with the proposed changes.

For the future, another solution could be to revert this PR:

https://github.com/robotology/icub-models-generator/pull/269

and the twins on robots-configuration and ergocub-software. In this way, the FT and the IMU would have the same name and the frame would be exported once for both.

cc @traversaro

traversaro commented 4 months ago

Good catch, the regression was in https://github.com/robotology/icub-models/commit/bdcf4e4c8f0da41db3e3d2643797ce1bff4d236e . In https://github.com/robotology/icub-models-generator/pull/276 I enabled some tests that we only had in iCub3 to detect these kind of problems, but they are passing, so probably there is some bug in the tests.

traversaro commented 4 months ago

I started by doing two PRs that are related to this problem, even if now they just cleanup the test infrastructure to make sure that we will detect similar problems in the future:

traversaro commented 4 months ago

Commit that fixed the problem: https://github.com/robotology/icub-models/commit/3faa9521782bf6e86278cac0be9464517f0766b0 .

robotology / icub-models-generator

simmechanics-to-urdf bug when exporting frames with the same link name #275