[torqueBalancing] Can the controller introduce self-maintained oscillations?

[S-Dafarra]

When performing the Yoga on the real robots it happens sometimes that the robot start shaking while balancing on single support.

Trying to figure out from where these oscillations my arise, we noticed that they come from, we noticed that the desired torques shows well sustained oscillations in one or more joints (for example the torso pitch and the ankle pitch) while the measured shows distorted and less frequent oscillations. This fact may show that indeed these vibrations may not be caused by the saturation of the F/Ts, or that at least they are not the only cause. An additional clue for this lead, is the following screenshot:

It has been taken from a successful (see no high frequency oscillations) Yoga extended demo on both feet. It is possible to notice that those estimated contact wrenches show a saturation (while balancing on the right foot), but the robot was still able to balance.

By further investigation we noticed that the estimated velocity of the base showed a big oscillation at very high frequency, which was then transmitted into the desired contact wrenches by the QP. Any idea on the source of these oscillations and how to get rid of them?

@gabrielenava @DanielePucci @traversaro @fiorisi @fjandrad

[traversaro]

How are we estimating the base velocity in recent versions of the controller?

[gabrielenava]

We assume one of the feet has a constant pose w.r.t. the inertial frame (the fixed foot is switched according to the yoga state). Then, knowing that the foot on ground velocity is zero, we write:

J(q)*nu = 0 [1]

Where nu is the state velocity and J (q) the foot jacobian. The joint velocity is given from measurements, the Jacobian is computed using WB toolbox and therefore the only unknown in [1] is the base velocity. To invert the part of the jacobian which multiplies the base velocity, we make use of a damped pseudoinverse.

[DanielePucci]

Probably @traversaro is referring to the existence of filters at the joint velocity level. In particular, if the flag readSpeedAccFromControlBoard is present in the yarpWholeBodyInterface.ini of the experimenting robot (see this file for iCubGenova02) then low pass filters are off, which may induce introduce noise, in turn inducing oscillations.

[fiorisi]

Probably now we can consider that, as seen during the last experiments, the YOGA++ works better (no oscillations nor vibrations) if we remove the FT feedback.

[gabrielenava]

I think I will close this issue. It will remain as archived knowledge, besides it has been a while since this problem occurred on the robot.