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

[martinaxgloria]

Working on:

• iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong – Stint 2 #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:

• Add left/right_arm FT IMU in iCubGazeboV2_* models #259
• iCubV2_* - Align IMU sensor names with the real robot ones #269

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:

• iCubV2_* - Align IMU sensor names with the real robot ones #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]

Good catch, the regression was in robotology/icub-models@bdcf4e4 . In #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]

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:

• Enable tests for consistency of FT sensor and "fake link" frames also to iCub v2 models #276
• Fix bug that caused iDynTree::ModelParserOptions to be ignored idyntree#1191

[traversaro]

Commit that fixed the problem: robotology/icub-models@3faa952 .