iCub >= V2.6 urdfs have the imu frame oriented as the V2.5

[Nicogene]

Today @martinaxgloria and I noticed that the imu_frame disappeared when I made the PR in icub-models-generator that exports the sensors frames:

• Uniform sensor name for IMU icub-models-generator#235

head_imu_0 took its place, but the problem is that this frame does not incorporate the additionalTransformation from the old imu frame (iCub v2.5) to the frame of the rfe.

You can see that in iCub 2.5 and 2.7 they have the same orientation.

icub-models/iCub/robots/iCubGazeboV2_5/model.urdf

Lines 986 to 992 in 453d9ed

	<link name="head_imu_0"/>
	<joint name="head_imu_0_fixed_joint" type="fixed">
	<origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
	<parent link="head"/>
	<child link="head_imu_0"/>
	<dynamics damping="0.1"/>
	</joint>

icub-models/iCub/robots/iCubGazeboV2_5/model.urdf

Line 2684 in 453d9ed

<pose>0.009500000190735 0.133444 0.009299999999999996 -1.5707963267948963 1.5707963267948963 0.0</pose>

icub-models/iCub/robots/iCubGazeboV2_7/model.urdf

Lines 986 to 992 in 453d9ed

	<link name="head_imu_0"/>
	<joint name="head_imu_0_fixed_joint" type="fixed">
	<origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
	<parent link="head"/>
	<child link="head_imu_0"/>
	<dynamics damping="0.1"/>
	</joint>

icub-models/iCub/robots/iCubGazeboV2_7/model.urdf

Line 2700 in 453d9ed

<pose>0.009500000190735 0.133444 0.009299999999999996 -1.5707963267948963 1.5707963267948963 0.0</pose>

If we read from the real imu and compare what we read in simulation we should notice this discrepancy

cc @traversaro @pattacini

[Nicogene]

Today I tried to export the urdf without the exportFrameInURDF, we obviously miss the head_imu_0 frame but we have the imu_frame link and the sensors seems oriented correctly.

V2_5

  <gazebo reference="head">
    <sensor name="head_imu_0" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.009500000190735 0.133444 0.009299999999999996 -1.5707963267948963 1.5707963267948963 0.0</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="head_imu_0" type="accelerometer">
    <parent link="head"/>
    <origin rpy="-1.5707963267948963 1.5707963267948963 0.0" xyz="0.009500000190735 0.133444 0.009299999999999996"/>
  </sensor>

V2_7

  <gazebo reference="head">
    <sensor name="head_imu_0" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.023453700190734995 0.20544400000000002 -0.023077900000000023 1.5707963267948997 -3.4914813388431334e-15 0.0</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="head_imu_0" type="accelerometer">
    <parent link="head"/>
    <origin rpy="1.5707963267948997 -3.4914813388431334e-15 0.0" xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023"/>
  </sensor>

So probably it is the exportFrameInURDF is somehow problematic.

• Add additionalTransform parameter to exportedFrames simmechanics-to-urdf#42
• Export ft sensor frames simmechanics-to-urdf#53

[Nicogene]

This PR fixes this problem:

• Include the additionalTransformation when the flag exportFrameInURDF is set to true for the sensors simmechanics-to-urdf#60

Unfortunately the exportFrameInURDF shadows the exportedFrames option, this is due that we are using as key of the map the CREO name that is in common between the two options, then now the sensor is correctly oriented, but imu_frame is missing.

On the other hand, imu_frame has to be kept in the yaml in order to express the additional transformation

[martinaxgloria]

So, the frame name for the imu in the urdf is still head_imu_0 after this fix but now contains the addition transform, right?

cc @Nicogene

[Nicogene]

So, the frame name for the imu in the urdf is still head_imu_0 after this fix but now contains the addition transform, right?

cc @Nicogene

Exaclty

V2_5

  <link name="head_imu_0"/>
  <joint name="head_imu_0_fixed_joint" type="fixed">
    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
    <parent link="head"/>
    <child link="head_imu_0"/>
    <dynamics damping="0.1"/>
  </joint>

V2_7

  <link name="head_imu_0"/>
  <joint name="head_imu_0_fixed_joint" type="fixed">
    <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="1.5707963267948997 0 0"/>
    <parent link="head"/>
    <child link="head_imu_0"/>
    <dynamics damping="0.1"/>
  </joint>

[martinaxgloria]

Great, thank you!

[Nicogene]

Closing since now V2_5 and V2_6/7 have the frames oriented differently