Pelvis orientation task contributes to balance control

[stephane-caron]

The current implementation has Controller update the two orientation tasks as follows:

    sva::PTransformd X_0_a = floatingBaseObs_.getAnchorFrame(controlRobot());
    pelvisOrientation_ = X_0_a.rotation();
    pelvisTask->orientation(pelvisOrientation_);
    torsoTask->orientation(mc_rbdyn::rpyToMat({0, torsoPitch_, 0}) * pelvisOrientation_);

This makes it look like orientation tasks are optional, which would be misleading. Here X_0_a is computed from control ankle frames, which comply with the ground. If the second line were

    pelvisOrientation_ = Eigen::Matrix3d::Identity();

then the upper body would sway as ankle angles propagate uncompensated to the hip.

The code should be refactored to make this compensation behavior explicit, making orientation tasks part of Stabilizer and adding the corresponding assumption to the spec.

[arntanguy]

I can confirm that this is problematic for lateral sway in my integration of your work in mc_rtc. I'll keep you posted when I have re-tested this on HRP2.

[stephane-caron]

This discussion is about a refactoring that wouldn't affect controller performance. It would have been better for a reader's understanding, yet it is now clear I will not do it 😉 So, instead, I have inlined this discussion in-code, and will now close this issue.

[stephane-caron]

Some more thoughts in a new wiki page about this question.