Deriving stabilizer equations

[Saeed-Mansouri]

@mehdi-benallegue @gergondet
I couldn't derive the stabilizer equations, especially additional terms. I have studied Murooka 2021 and Morisawa 2012, however, the equations are not matched with these papers. I will appreciate it if you help me to derive the following equations.

/// feed forward
Eigen::Vector3d desiredCoMAccel = comddTarget_;

/// Proportional term
Eigen::Vector3d gain = Eigen::Vector3d{c_.dcmPropGain.x(), c_.dcmPropGain.y(), 0};
desiredCoMAccel += omega_ * R_0_fb_yaw.transpose() * gain.cwiseProduct(R_0_fb_yaw * dcmError_);

/// integral term
gain = Eigen::Vector3d{c_.dcmIntegralGain.x(), c_.dcmIntegralGain.y(), 0};
desiredCoMAccel += omega_ * R_0_fb_yaw.transpose() * gain.cwiseProduct(R_0_fb_yaw * dcmAverageError_);

/// derivative term
gain = Eigen::Vector3d{c_.dcmDerivGain.x(), c_.dcmDerivGain.y(), 0};
desiredCoMAccel += omega_ * R_0_fb_yaw.transpose() * gain.cwiseProduct(R_0_fb_yaw * dcmVelError_);

/// additional terms
desiredCoMAccel += omega_ * (c_.comdErrorGain * comdError);
desiredCoMAccel -= omega_ * omega_ * c_.zmpdGain * zmpdTarget_;

[mmurooka]

From my understanding, the below term is unnecessary. I introduced comdErrorGain to disable this term and set it to zero when I use StabilizerTask.

desiredCoMAccel += omega_ * (c_.comdErrorGain * comdError);

The below term is used in the previous controller in AIST, and I added it to the mc_rtc's StabilizerTask. This has the effect of compensating for the delay in foot damping control and improves ZMP tracking performance in the StabilizerTask. Equation (10) in this paper corresponds to this term.

desiredCoMAccel -= omega_ * omega_ * c_.zmpdGain * zmpdTarget_;