-
Notifications
You must be signed in to change notification settings - Fork 23
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
Comments
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? |
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? |
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 |
Yesterday, while running the test-multiple-imu-icubgenova11.webmThis 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. |
Can you report the config file you used for that test? |
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! |
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. |
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 <?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 |
Good catch! |
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: 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 ? |
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: |
Together with @Nicogene, we manually modified the urdf to add the <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.webmIn 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 |
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. |
Alternatively, we can fix it on the
We applied the <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> |
Yes, this make sense.
I do not think iDynTree parses that (the format uses in iDynTree is documented, let me check that! |
Do you mean |
Yes. |
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
The proposed fix: Seems to fix the ft sensors measurements, but does not change what happens to the imu attached to those frames |
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. |
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
Running
iCubGazeboV2_*
model and visualizing it inrviz2
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?
icub-models-generator/tests/icub-model-test.cpp
Lines 442 to 466 in 1a5d6b4
cc @pattacini
The text was updated successfully, but these errors were encountered: