Bug in foot force difference control

[Saeed-Mansouri]

Hi Dr. Caron,

As far as I understood, in the foot force difference control, the target and measured forces are represented in the sole frames. However, the target and surface positions of the foot are in the inertial frame. I have the following questions:

1. The vertical drift compensation term is in the inertial frame. However, the respective velocities are in the sole frames. Does it make sense?

2. In the damping term (Eq 19 of the paper), the left and right feet' vertical forces are not in the same frame. How can we sum two variables represented in the different frames?

3. Does the current foot force difference control work for the non-planar contacts?

[stephane-caron]

Excellent catch! 🎣 This is a big bug, thank you for pointing it out 😀

The vertical drift compensation term is in the inertial frame. However, the respective velocities are in the sole frames. Does it make sense?

No it doesn't 😊 Similarly to jrl-umi3218#29 there is a missing transformation here. All quantities should be in the same frame.

In the damping term (Eq 19 of the paper), the left and right feet' vertical forces are not in the same frame. How can we sum two variables represented in the different frames?

We can't, foot forces should be transformed to the world frame for these calculations to make sense. In practice it went unnoticed because we were either walking or stair climbing, where surfaces are horizontal and foot frame normal vectors are roughly vertical all the time.

Does the current foot force difference control work for the non-planar contacts?

It works for non-coplanar contacts, but the contacts need to be horizontal. If contacts are tilted you can go for the more general idea of doing admittance control in the nullspace of the contact Jacobian, like we are discussing in #74.

[mmurooka]

FYI, the similar changes as jrl-umi3218/mc_rtc#289 should fix the issue.