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_;