iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong

Nicogene commented 1 year ago

Running iCubGazeboV2_* model and visualizing it in rviz2 I noticed that the ft measurement for the arms ar odd (FT frames exportation has been introduced in #232) , the ones for the hips and legs seems fine instead.

On iCubGazeboV3 also the ft frames in the arms seem correct.

@traversaro I remember that in the past we already discussed about the frames' definitions for iCubGazeboV2_* models that there were something odd but I forgot.

There is a check about the ft sensor orientation in the unit tests, but may this check be wrong?

https://github.com/robotology/icub-models-generator/blob/1a5d6b4e1a680a2d7fffea939e6dbf186acabb7d/tests/icub-model-test.cpp#L442-L466

❌ arms	✔️ hips	✔️ feet

cc @pattacini

traversaro commented 1 year ago

It is not clear to me from what in the image do you see that the orientation is incorrect. I cannot see the frame from the image, just the arrow. Are you sure that is not the force reading that is not correct?

traversaro commented 1 year ago

Can you try to force the rendering in rviz2 to have all the joint in zero so we are in the case of the icub-models unit tests?

Nicogene commented 1 year ago

Looking this diff seems that the ft frames of the arms are attached to the link with a fixed joint but with a transform different from <origin xyz="0 0 0" rpy="0 0 0"/> like the ones of the hips and feet, and this is suspicious

Nicogene commented 1 year ago

I tried to visualize only the frames of the ft sensors and respective parent-child links

left arm	right arm

It seems that between the right and left the ft frames are differently oriented and the measurement seems to be not along the correct axis.

For example here is what happens in the left hip

immagine

martinaxgloria commented 7 months ago

Yesterday, while running the imu test we are implementing (see here for reference) on iCubGenova11, we noticed that the expected orientation measurements retrieved from the IMU of the FT sensors on the arms do not match the measured ones.

test-multiple-imu-icubgenova11.webm

This could be related to what @Nicogene saw on rviz and reported in this issue. We could deeply investigate if there's a mismatch between the real robot and the CAD, so the urdf.

cc @traversaro @pattacini

traversaro commented 7 months ago

Yesterday, while running the imu test we are implementing (see here for reference) on iCubGenova11, we noticed that the expected orientation measurements retrieved from the IMU of the FT sensors on the arms do not match the measured ones.

Can you report the config file you used for that test?

traversaro commented 7 months ago

Not sure if you are able to log the data (i.e. joint positions and imu output) in the tests, but if you did it would be convenient to attach it to the issue, thanks!

martinaxgloria commented 7 months ago

Yesterday, while running the imu test we are implementing (see here for reference) on iCubGenova11, we noticed that the expected orientation measurements retrieved from the IMU of the FT sensors on the arms do not match the measured ones.

Can you report the config file you used for that test?

Do you mean the configuration file of the test or on the robot side?

traversaro commented 7 months ago

Yesterday, while running the imu test we are implementing (see here for reference) on iCubGenova11, we noticed that the expected orientation measurements retrieved from the IMU of the FT sensors on the arms do not match the measured ones.

Can you report the config file you used for that test?

Do you mean the configuration file of the test or on the robot side?

Of the test. For robot/model it probably is sufficient if you can report the version and/or git checkout of icub-models and robots-configuration.

martinaxgloria commented 7 months ago

Of the test. For robot/model it probably is sufficient if you can report the version and/or git checkout of icub-models and robots-configuration.

icub-models is on the last devel https://github.com/robotology/icub-models/commit/e9d33b61f36955a8da238c2c06389816c3c1ec85
robots-configuration is on the last devel too, but I added (locally so far, but I will open a PR to have those files upstream) the configuration files under /hardware/inertials for both the left_arm and right_arm, like this one.

Moreover, this is the configuration file used for the test:

robot                   ${robotname}
model                   model.urdf
port                    /${robotname}/alljoints/FT
remoteControlBoards     ("torso", "head", "left_arm", "right_arm")
axesNames               ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw")
testList                ("head_imu_0", "l_arm_ft", "r_arm_ft")
meanError               0.1

[head_imu_0]
moveJoints ("neck_pitch", "neck_roll")

[l_arm_ft]
moveJoints ("l_shoulder_pitch")

[r_arm_ft]
moveJoints ("r_shoulder_pitch")

In this sense, on the robot side a multipleanalogsensorsremapper is used to remap the inertial of the FTs, while on the test side a multipleanalogsensorsclient is opened to read from those sensors. This is the alljoints-FT_remapper.xml (the name is temporary):

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-FT_remapper" type="multipleanalogsensorsremapper">
        <param name="OrientationSensorsNames">
            (l_arm_ft r_arm_ft head_imu_0)
        </param>
        <action phase="startup" level="5" type="attach">
            <paramlist name="networks">
                <elem name="head_imu">  head_inertial </elem>
                <elem name="left_arm_imu">  left_arm_inertials </elem>
                <elem name="right_arm_imu">  right_arm_inertials </elem>
            </paramlist>
        </action>
        <action phase="shutdown" level="20" type="detach" />
    </device>

It could be probably cleared after I open the PR versus robots-configuration with all the configuration files involved

traversaro commented 7 months ago

Thanks, that is much more clear. Something that I noticed that it is strange for the arm sensors is that if you check the leg sensors, for example in:

As the sensor frame origin is coincident by definition in the CAD with the l_hip_3 link frame origin (see https://github.com/icub-tech-iit/cad-libraries/wiki/Prepare-PTC-Creo-Mechanism-for-URDF#preliminary-operations), we can see that as expected the translation offset of all the definition of the FT sensor frame are 0.

On the other hand, the l_arm_ft sensor should be aligned with the origin of the r_upper_arm link frame, but that is not the case: https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/robots/iCubGenova11/model.urdf#L2682

I guess something is broken there.

Nicogene commented 7 months ago

Thanks, that is much more clear. Something that I noticed that it is strange for the arm sensors is that if you check the leg sensors, for example in:
* [robotology/icub-models@`8364409`/iCub/robots/iCubGenova11/model.urdf#L2584](https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/robots/iCubGenova11/model.urdf#L2584)

* [robotology/icub-models@`8364409`/iCub/robots/iCubGenova11/model.urdf#L2594](https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/robots/iCubGenova11/model.urdf#L2594)

* [robotology/icub-models@`8364409`/iCub/robots/iCubGenova11/model.urdf#L128](https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/robots/iCubGenova11/model.urdf#L128)
As the sensor frame origin is coincident by definition in the CAD with the l_hip_3 link frame origin (see Wiki: Prepare PTC Creo Mechanism for URDF (preliminary operations) (icub-tech-iit/cad-libraries)), we can see that as expected the translation offset of all the definition of the FT sensor frame are 0.

On the other hand, the l_arm_ft sensor should be aligned with the origin of the r_upper_arm link frame, but that is not the case: robotology/icub-models@8364409/iCub/robots/iCubGenova11/model.urdf#L2682

I guess something is broken there.

Good catch! In fact for ergocub the linkName is l_shoulder_2 because the ft joint is defined between l_shoulder_2 --> l_shoulder_3

Nicogene commented 7 months ago

Today @martinaxgloria and I analyzed how the csys of the arms' FT are defined in the simulation model cad, and it is seems coherent with this image:

immagine

So the orientation seems fine, the only strange thing we noticed is that that CSYS is defined ONLY in the parent link of the ft joint, in the child link it is missing.

Probably this is why the frame is exported referencing the parent link:

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

Maybe is this "workaround" the source of error ?

cc @fiorisi @pattacini @traversaro

martinaxgloria commented 7 months ago

Moreover, trying to visualize the FT frames on rviz, we noticed that the orientations are coherent with the CAD but the measures of force and torques seem very odd:

right_arm
left_arm

cc @Nicogene @traversaro @fiorisi @pattacini

Nicogene commented 6 months ago

Looking at the sdf specification we have that the pose of the sensor is defined as follow: immagine

Since relative_to is not defined, it is assumed that is the parent of the xml -> the ft joint frame -> the child link frame. Unfortunately, since the csys is only in the parent link, probably that pose is referred to the parent link frame, so I would give a try specifying relative_to="l_shoulder_3" and if this work we can add the ft sensors of the arms as xml blob for avoiding the creo9->7 procedure

cc @martinaxgloria @traversaro

martinaxgloria commented 6 months ago

Together with @Nicogene, we manually modified the urdf to add the relative_to attribute in both the l_arm_ft_sensor and r_arm_ft_sensor:

  <gazebo reference="l_arm_ft_sensor">
    <sensor name="l_arm_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose relative_to="l_shoulder_3">0.05093014324520106 1.3877787807814457e-17 0.029175829465900395 -4.3625653762663687e-16 -1.3089970069677406 -3.1415926535897922</pose>
      <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="l_arm_ft" type="force_torque">
    <parent joint="l_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <pose relative_to="l_shoulder_3">0.05093014324520106 1.3877787807814457e-17 0.029175829465900395 -4.3625653762663687e-16 -1.3089970069677406 -3.1415926535897922</pose>
  </sensor>
  <gazebo reference="r_arm_ft_sensor">
    <sensor name="r_arm_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose relative_to="r_shoulder_3">-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215 -6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16</pose>
      <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="r_arm_ft" type="force_torque">
    <parent joint="r_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <pose relative_to="r_shoulder_3">-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215 -6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16</pose>
  </sensor>

and we saw the result in rviz:

Screencast from 03-13-2024 12:35:53 PM.webm

In our opinion, looking at the arrows representing the forces, it seems to be more coherent now. We could do the same for the IMUs and test them with the IMU test.

cc @traversaro

traversaro commented 6 months ago

Great job! However, what about the non-Gazebo sensor tag? Unfortunately that will remain wrong? I am in mobile so I can't easily link any code, but unfortunately that is the tag used by iDynTree (and hence wholeBodyDynamics) to read the FT sensor pose.

Nicogene commented 6 months ago

Together with @Nicogene, we manually modified the urdf to add the relative_to attribute in both the l_arm_ft_sensor

Alternatively, we can fix it on the simmechanics_to_urdf side, we should keep track of frameReferenceLink and export the pose wrt that link. But maybe it is more convenient to add them as xml blobs since simmechanics_to_urdf has been kinda of frozen for the creo9 issue.

what about the non-Gazebo sensor tag

We applied the relative_to also to the non-gazebo sensor tag, see:

  <sensor name="r_arm_ft" type="force_torque">
    <parent joint="r_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <pose relative_to="r_shoulder_3">-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215 -6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16</pose>
  </sensor>

traversaro commented 6 months ago

Together with @Nicogene, we manually modified the urdf to add the relative_to attribute in both the l_arm_ft_sensor

Alternatively, we can fix it on the simmechanics_to_urdf side, we should keep track of frameReferenceLink and export the pose wrt that link. But maybe it is more convenient to add them as xml blobs since simmechanics_to_urdf has been kinda of frozen for the creo9 issue.

Yes, this make sense.

what about the non-Gazebo sensor tag

We applied the relative_to also to the non-gazebo sensor tag, see:

  <sensor name="r_arm_ft" type="force_torque">
    <parent joint="r_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <pose relative_to="r_shoulder_3">-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215 -6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16</pose>
  </sensor>

I do not think iDynTree parses that (the format uses in iDynTree is documented, let me check that!

traversaro commented 6 months ago

https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#force_torque

Nicogene commented 6 months ago

I do not think iDynTree parses that

Do you mean relative_to?

traversaro commented 6 months ago

I do not think iDynTree parses that

Do you mean relative_to?

Yes.

Nicogene commented 6 months ago

The proposed fix:

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

Seems to fix the ft sensors measurements, but does not change what happens to the imu attached to those frames

https://github.com/robotology/icub-models-generator/issues/233#issuecomment-1945584543

traversaro commented 6 months ago

The proposed fix:

* [iCubV2_*: Fix measure of arm ft sensors #265](https://github.com/robotology/icub-models-generator/pull/265)

Seems to fix the ft sensors measurements, but does not change what happens to the imu attached to those frames

* [iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong #233 (comment)](https://github.com/robotology/icub-models-generator/issues/233#issuecomment-1945584543)

I guess that make sense, as I tried to see the changes caused by that PR, and the change are just:

diff --git a/iCub/robots/iCubGazeboV2_7/model.urdf b/iCub/robots/iCubGazeboV2_7/model.urdf
index 3992313..76e6893 100644
--- a/iCub/robots/iCubGazeboV2_7/model.urdf
+++ b/iCub/robots/iCubGazeboV2_7/model.urdf
@@ -2654,7 +2654,7 @@
       <always_on>1</always_on>
       <update_rate>100</update_rate>
       <force_torque>
-        <frame>sensor</frame>
+        <frame>child</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
       <pose>0.05093014324520106 1.3877787807814457e-17 0.029175829465900395 -4.3625653762663687e-16 -1.3089970069677406 -3.1415926535897922</pose>
@@ -2666,7 +2666,7 @@
   <sensor name="l_arm_ft" type="force_torque">
     <parent joint="l_arm_ft_sensor"/>
     <force_torque>
-      <frame>sensor</frame>
+      <frame>child</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
     <origin rpy="-4.3625653762663687e-16 -1.3089970069677406 -3.1415926535897922" xyz="0.05093014324520106 1.3877787807814457e-17 0.029175829465900395"/>
@@ -2676,7 +2676,7 @@
       <always_on>1</always_on>
       <update_rate>100</update_rate>
       <force_torque>
-        <frame>sensor</frame>
+        <frame>child</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
       <pose>-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215 -6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16</pose>
@@ -2688,7 +2688,7 @@
   <sensor name="r_arm_ft" type="force_torque">
     <parent joint="r_arm_ft_sensor"/>
     <force_torque>
-      <frame>sensor</frame>
+      <frame>child</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
     <origin rpy="-6.358700866473837e-16 -1.3089970069677404 6.58301142551668e-16" xyz="-0.05089730176650909 1.3877787807814457e-17 0.029167029620599215"/>

so they are not related to the IMU sensor in any way.

robotology / icub-models-generator

iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong – Stint 1 #233