Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed
Nicogene opened this issue Jan 20, 2023 · 24 comments · Fixed by #265
Assignees

Comments

@Nicogene
Copy link
Member

Nicogene commented Jan 20, 2023

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?

bool checkFTSensorsAreCorrectlyOrientedV2(iDynTree::KinDynComputations & comp)
{
// The rotation of all the F/T sensors in the iCub robot when in zero position are the same
iDynTree::Rotation rootLink_R_sensorFrameExpected =
iDynTree::Rotation(-1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
// The rotation of the foot F/T sensors in the iCub 2.5+ is different
iDynTree::Rotation rootLink_R_foot_sensorFrameExpected_plus =
iDynTree::Rotation(-0.965926, 0.0, 0.258819,
0.0, 1.0, 0.0,
-0.258819, 0.0, -0.965926);
bool isPlusModel = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_foot_sensorFrameExpected_plus, "l_foot_ft_sensor")
&& checkFTSensorIsCorrectlyOriented(comp, rootLink_R_foot_sensorFrameExpected_plus, "r_foot_ft_sensor");
bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_arm_ft_sensor");
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_arm_ft_sensor") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_leg_ft_sensor") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_leg_ft_sensor") && ok;
ok = (checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_foot_ft_sensor") || isPlusModel) && ok;
ok = (checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_foot_ft_sensor") || isPlusModel) && ok;
return ok;
}

❌ arms ✔️ hips ✔️ feet
immagine immagine immagine

cc @pattacini

@traversaro
Copy link
Member

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
Copy link
Member

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
Copy link
Member Author

Nicogene commented Jan 20, 2023

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
Copy link
Member Author

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

left arm right arm
immagine immagine

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
Copy link
Collaborator

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
Copy link
Member

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 traversaro changed the title iCubGazevoV2_*: the frames of the ft sensors in the arms seems wrong iCubGazeboV2_*: the frames of the ft sensors in the arms seems wrong Feb 15, 2024
@traversaro
Copy link
Member

traversaro commented Feb 15, 2024

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
Copy link
Collaborator

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
Copy link
Member

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.

@traversaro traversaro changed the title iCubGazeboV2_*: the frames of the ft sensors in the arms seems wrong iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong Feb 15, 2024
@martinaxgloria
Copy link
Collaborator

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 robotology/icub-models@e9d33b6
  • 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
Copy link
Member

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
Copy link
Member Author

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
Copy link
Member Author

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

immagine

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:

Maybe is this "workaround" the source of error ?

cc @fiorisi @pattacini @traversaro

@martinaxgloria
Copy link
Collaborator

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

    Screenshot from 2024-03-01 15-13-24

  • left_arm

    Screenshot from 2024-03-01 15-12-51

cc @Nicogene @traversaro @fiorisi @pattacini

@Nicogene
Copy link
Member Author

Nicogene commented Mar 13, 2024

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
Copy link
Collaborator

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
Copy link
Member

traversaro commented Mar 13, 2024

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
Copy link
Member Author

Nicogene commented Mar 13, 2024

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
Copy link
Member

traversaro commented Mar 13, 2024

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
Copy link
Member

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

@Nicogene
Copy link
Member Author

I do not think iDynTree parses that

Do you mean relative_to?

@traversaro
Copy link
Member

I do not think iDynTree parses that

Do you mean relative_to?

Yes.

Nicogene added a commit that referenced this issue Mar 14, 2024
It fixes #233.

Basically the problem is that in the child link of the ft sensor joints of the arms we do not have the CSYS, then we are taking it from the parent link. Putting `frame` to `child` make that the `pose` is referring to the correct frame
@Nicogene
Copy link
Member Author

Nicogene commented Mar 14, 2024

The proposed fix:

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

@traversaro
Copy link
Member

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.

traversaro pushed a commit that referenced this issue Mar 14, 2024
It fixes #233.

Basically the problem is that in the child link of the ft sensor joints of the arms we do not have the CSYS, then we are taking it from the parent link. Putting `frame` to `child` make that the `pose` is referring to the correct frame
@Nicogene Nicogene changed the title iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong – Stint 1 May 17, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment