Stance leg knee is bending while performing Yoga++ demo

[gabrielenava]

While performing Yoga++ demo on iCubGenova04, the stance leg knee joint started bending. It happened when the robot was on one foot. It seems the problem is related to the configuration defined in the file wholebodydynamics.xml. In particular, the "standard" configuration was not causing the knee to bend, while the configuration used for standing up the robot was.

Here is the result of running diff command between the two configurations (symbol < denotes the stand up configuration while > indicates the standard one):

9c9
<         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,r_upper_leg,l_upper_leg,l_elbow_1,r_elbow_1)</param>
---
>         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,r_lower_leg,l_lower_leg,l_elbow_1,r_elbow_1)</param>
11a12
>       <param name="forceTorqueEstimateConfidence">2</param>
19a21,22
<             <param name="l_upper_leg">(l_upper_leg,5,2)</param>
29d28
<             <param name="r_upper_leg">(r_upper_leg,6,2)</param>
40c39
<       <!--    <group name="FT_SECONDARY_CALIBRATION">
---
>         <group name="FT_SECONDARY_CALIBRATION">
55c54,55
<           </group> -->
---
>           </group>
>
57,58d56
<             <param name="/wholeBodyDynamics/left_arm/endEffectorWrench:o">(l_hand,l_hand_dh_frame,l_sole)</param>
<             <param name="/wholeBodyDynamics/right_arm/endEffectorWrench:o">(r_hand,r_hand_dh_frame,l_sole)</param>

[gabrielenava]

cc @francesco-romano @traversaro

[gabrielenava]

I have the results of Joint Torque Estimation analysis.

WARNING: Plots are not fancy, I will optimize figures later.

• There were no big differences between the estimated torso and arms joint torques. However, looking at the legs, there is a clear difference in the estimated knee torques when balancing on left foot, and between all leg torques when balancing on two feet:

LEFT FOOT:

TWO FEET:

• However, the difference disappear while the robot is in the air:

• The results while balancing on two feet seem influenced by internal stresses, but however I think we may trust the result on the knees while balancing on left foot. Hence, to me this analysis confirmed that something is wrong with the knee torque estimation.

• Data while balancing on the right foot were corrupted and therefore not usable.

• Matlab code used for plotting data: tests_13_11_iCubGenova02.zip

[fiorisi]

Thanks @gabrielenava , as we were discussing also with @fjandrad, probably we can do the same test in simulation (reading the torques from WBD and not from GAZEBO). If there is no difference then this problem is a consequence of phantom forces and not an issue with WBD. Finally, let's also see the results of the tests on iCubGenova04.

[gabrielenava]

We performed on iCubGenova04 the following experiments:

• the robot was balancing on left foot, in position control. In the meanwhile, we evaluated the estimated joint torques in 4 different configurations:

WBD default contact frame	calibration type	number of exp.
lower leg	on air	3
lower leg	on ground	3
upper leg	on air	3
upper leg	on ground	2

the critical joints is the left leg joints (see https://github.com/robotology-playground/WBI-Toolbox-controllers/issues/56#issuecomment-346072035). These are the torques on the left leg joints for all the experiments:

as expected, there is a difference between the estimated torques while calibration is performed on air (blue and red lines) and the estimated torques while calibration is performed on ground (green and black). There is also a difference in the estimated left knee torque considering as contact frame the lower leg (blue) or the upper leg (red) while calibration is performed on air, but the difference is negligible while calibration is performed on ground.

This is consistent with our explanation of the phenomenon: there are no bugs in torques estimation, but the presence of phantom forces in the stance leg induces a difference in the estimation of the knee torque depending on which contact frame is used by WBD. This difference is reduced while calibration is performed on ground, because the calibration offsets are hiding this phenomeon.

• Knowing that this behaviour is consistent with how WBD works, we can close this issue.