[iCubGazeboV3] Wholebody dynamics retuns high contact force on the hands while walking

[GiulioRomualdi]

I ran a walking experiment on iCubGazeboV3. I also ran whole-body dynamics to retrieve the measured contact wrenches on the hands.

YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml

Even if no external forces were acting on the system (excepts on the feet) and the robot walks fine, the forces retrieved by whole-body dynamics are pretty high.

Any idea why this is happening?

cc @traversaro @S-Dafarra @prashanthr05

[traversaro]

Any idea why this is happening?

cc @traversaro @S-Dafarra @prashanthr05

At a first glance, I would say that the measures that we get from Gazebo do not fit the nominal model used for force estimation (for example due to numerical simulation errors) or perhaps the wbd filters are introducing errors w.r.t. to the measured quantities that result in a discrepancy that is reflected as an estimated external force. I will add to this issue a more in-depth description that should help anyone that wants to debug this.

[traversaro]

So, first of all let recap how the external 6D force/torque on the hand is computed (please install a extention that renders LaTeX to see this comment, such as
).

[traversaro]

The main reference text for the algorithm underlying wholebodydynamics is the Chapter 4 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf . In particular, the equation that describes how external forces are computed in case there is only one unknown external force/torque for each submodel is Equation 4.21 :

$$ {}_{C} \mathrm{f}^x_{C} = \sum_{L \in \mathfrak{L}_{sm}} {}_C X^L {}_L \phi_L - \sum_{L \in \mathfrak{L}_{sm}} \sum_{D \in \beth_{sm}(L)} {}_C X^D {}_D \mathrm{f}_{D,L} . $$

where:
${}_C X^L {}_L \phi_L$ is the component of the force that depends on inertial parameters (due to inertial effects and gravity), defined by Equation 4.9 :
$$
{}_B \phi_B( {}^B \alpha^g_{A,B}, {}^B \omega_{A,B}) := {}_B \mathbb{M}_B \alpha^g_{A,B} + \begin{bmatrix}
0_{3\times1} \\
\omega_{A,B}
\end{bmatrix}
\bar{\times}^*
{}_B \mathbb{M}_B
\begin{bmatrix}
0_{3\times1} \\
{}^B \omega_{A,B}
\end{bmatrix}
$$

while the other term is the force measured by the F/T sensors connected to the submodels.

In the specific case of icub arm, this means that the external force on the l_hand link depends on:

• The force that depends on inertial parameters, acceleration and velocity for the links that belong to the arm sub-model, i.e.:
  • l_hand
  • l_wrist_1
  • l_forearm
  • l_elbow_1
  • l_upper_arm
  • l_shoulder_3
• The force measured by the l_arm_ft_sensor FT sensors

In particular, the "force that depends on inertial parameters, acceleration and velocity" depends on the sensor proper acceleration ${}_B \phi_B( {}^B \alpha^g_{A,B}, {}^B \omega_{A,B})$ and on the angular velocity ${}^B \omega_{A,B}$ and on the inertial parameters. The inertial parameters are assumed to be constant, so those are hardly the problem, even because the force estimation is correctly zero when the robot is not moving. On the other hand, ${}_B \phi_B( {}^B \alpha^g_{A,B}, {}^B \omega_{A,B})$ are computed using the measure of one IMU on the robot (either head or base, depending on the configuration), that is then propagated to all other links using the measured joint position, velocity and acceleration for all the joints between the link of interest and the link that has IMU (see section 4.4.3 of the thesis).

For this reason, it would be interesting to change a bit the estimation parameters to understand what is going wrong. Some easy checks are:

• Increase the filter frequency by setting the parameters imuFilterCutoffInHz, forceTorqueFilterCutoffInHz, jointVelFilterCutoffInHz, jointAccFilterCutoffInHz to high values such as 1e6 , see https://github.com/robotology/whole-body-estimators/blob/035d96ef6c48ac3a07d53b4914cf6a1233b8d5c1/devices/wholeBodyDynamics/README.md#wholebodydynamicsdevice . Probably for doing that it would be more convenient to have a parameter to disable filtering at all, see wholebodydynamics: Add option to disable all filtering in input measures #121 .
• It may be also worth to keep filter rather conservative an instead disable the joint velocity and joint acceleration feedback, as it may be that this measures are noisy. To do so, you can set useJointVelocity and useJointAcceleration to false.

If the estimation is always noisy even by changing this parameters, probably it may be a good idea to investigate instead the inputs to the estimation process (IMU measures, FT sensor measures, joint position, velocity and acceleration), especially as some of those are known to be noisy when using default Gazebo/ODE parameters.

[traversaro]

fyi @prashanthr05 @isorrentino @HosameldinMohamed

[traversaro]

Another useful simpler test is to do this test in the fixed base case. If the problem is also there, it may be much easier to debug due to the presence of a single external force, and the fact that the base position, velocity and acceleration are fixed, so we can compute an appropriate ground truth by taking the position values, filter them and compute by numerical differences the velocity and acceleration, and then compute FT sensor measures and internal torques via inverse dynamics. This is a test that we can easily do on the real robot as well, both iCub 2.* or iCub 3.

[traversaro]

Another useful simpler test is to do this test in the fixed base case. If the problem is also there, it may be much easier to debug due to the presence of a single external force, and the fact that the base position, velocity and acceleration are fixed, so we can compute an appropriate ground truth by taking the position values, filter them and compute by numerical differences the velocity and acceleration, and then compute FT sensor measures and internal torques via inverse dynamics. This is a test that we can easily do on the real robot as well, both iCub 2.* or iCub 3.

By the way, this is a test that can be easily done also on the real robot. @HosameldinMohamed @isorrentino

[HosameldinMohamed]

@SanderRuijters