Wrong estimation of torso pitch torque

[fils99]

Today, carrying out some experiments on ergoCubSN001, we noticed a strange behavior on torso pitch joint. Setup of the experiment:

• The robot was hanging on the crane, without touching ground
• Whole-Body-Dynamics was launched without considering the FTs (leveraging the work done by @traversaro in https://github.com/robotology/whole-body-estimators/pull/194

Below, we show the estimated torque on the torso pitch during the experiment, in which we were driving such joint from a joint limit to the other one, in position control.

experiment.webm

It seems strange because the range of variation of the estimated joint torque is quite small, considering that we reach the two limit joint positions. Moreover, it is always positive. In the meanwhile, as we can se by the plot, the current changes sign.

cc @LoreMoretti @GiulioRomualdi

[GiulioRomualdi]

To be fair we also checked the IMU orientation and it seems fine, there is a slight error on the position in the urdf (@fils99, you may link the issue here). But I don't think this is the cause of this issue.

[fils99]

To be fair we also checked the IMU orientation and it seems fine, there is a slight error on the position in the urdf (@fils99, you may link the issue here). But I don't think this is the cause of this issue.

Here the issue https://github.com/icub-tech-iit/ergocub-software/issues/260#issue-2519170084

[GiulioRomualdi]

@traversaro mentioned via teams that we should set the default contact frame in the chest and not in the root link in wbd config file

[fils99]

@traversaro mentioned via teams that we should set the default contact frame in the chest and not in the root link in wbd config file

So we have to set default contact frame in the chest every time we exclude at least one FT sensor? Or only when we exclude all the FTs?

[traversaro]

The point Is:

The robot was hanging on the crane, without touching ground

If the robot Is hanging on the crane, the external force is on the chest, not ok the root_link, independently from the FTs used.

[traversaro]

It would be easier to understand if we visualized all the external forces published on the contacts:o port, not now but probably we should think of visualized that in some visualizer.

[fils99]

Trial 1

We tried to put default contact frame in the chest. Below the results:

1.webm

As we can see from the video, the behavior of the comment above persists. Moreover, with idyntree-yarp-visualizer we noticed also a strange force acting on the left hand of the robot.

Visualizer


Most likely is the force of traction of the crane on the robot (?), but why is it placed on the hand?

Trial 2

Commit: https://github.com/ami-iit/robots-configuration/commit/f5d7a602edd6f6fe5627940735f7b3d173f0e44a

In this case, we are substituting every istance of root_link present in the file with chest (so, not only in the parameter defaultContactFrames).

Below the results

2.webm

On this time, the behavior of the torso pitch estimated torque seems more reasonable.

Btw, with idyntree-yarp-visualizer we still see the strange force acting on the left hand of the robot. Is it a problem of WBD or idyntree-yarp-visualizer :

Visualizer


[traversaro]

If you do not have any ft, then the only contact frame should be the chest one.

[fils99]

So in defafultContactFrames should I remove also l_hand_palm, l_foot_front and so on, right?

[fils99]

Problem Fixed

Today I modified the config file according to SIlvio's suggestion, and everything seem fine, both in the case of zero FTs considered and also in the case in which we use the feet FTs.

No FTs

Configuration wbd

```xml (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) model.urdf (0,0,-9.81) (chest) waist_imu_0 true 2 true false true 3.0 10.0 10.0 3.0 true 0.002 false true root_link waist_imu_0 waist_imu_0 () true root_link (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,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll) (l_hand_palm,l_hand_palm,root_link) (r_hand_palm,r_hand_palm,root_link) (l_lower_leg,l_lower_leg,root_link) (r_lower_leg,r_lower_leg,root_link) (l_foot_front,l_sole,l_sole) (l_foot_rear,l_sole,l_sole) (r_foot_front,r_sole,r_sole) (r_foot_rear,r_sole,r_sole) head-eb20-j0_1-mc head-eb21-j2_3-mc torso-eb5-j0_2-mc left_arm-eb2-j0_1-mc left_arm-eb4-j2_3-mc left_arm-eb31-j4_6-mc right_arm-eb1-j0_1-mc right_arm-eb3-j2_3-mc right_arm-eb30-j4_6-mc left_leg-eb8-j0_3-mc left_leg-eb9-j4_5-mc right_leg-eb6-j0_3-mc right_leg-eb7-j4_5-mc waist-inertial left_leg-FT_remapper right_leg-FT_remapper left_arm-FT_secondary_calib right_arm-FT_secondary_calib ```

Visualizer


With feet FTs

Commit: https://github.com/ami-iit/robots-configuration/commit/c5274979c2c91cb42ca1d8f80bd7b2a2c59a9926

Visualizer


Conclusions

In both the cases analyzed, when the robot is hanging on the crane, the force (and the related moment) are applied in the right place (i.e. the chest)

[traversaro]

Exactly, we can align in person to explain more how wbd algorithm work.

[fils99]

It's good to me, thanks!