Incorrect PD Feedback Calculation in computeRbdTorqueFromCentroidalModelPD Function

[Renkunzhao]

Describe the bug
I have identified an issue within the computeRbdTorqueFromCentroidalModelPD function, where the Proportional-Derivative (PD) feedback calculation appears to be incorrectly implemented.

vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(const vector_t& desiredState, const vector_t& desiredInput,
                                                                              const vector_t& desiredJointAccelerations,
                                                                              const vector_t& measuredRbdState, const vector_t& pGains,
                                                                              const vector_t& dGains) {
  // handles
  const auto& info = mapping_.getCentroidalModelInfo();
  const auto& model = pinocchioInterface_.getModel();
  auto& data = pinocchioInterface_.getData();

  // desired
  Vector6 desiredBasePose, desiredBaseVelocity, desiredBaseAcceleration;
  computeBaseKinematicsFromCentroidalModel(desiredState, desiredInput, desiredJointAccelerations, desiredBasePose, desiredBaseVelocity,
                                           desiredBaseAcceleration);
  vector_t qDesired(info.generalizedCoordinatesNum), vDesired(info.generalizedCoordinatesNum), aDesired(info.generalizedCoordinatesNum);
  qDesired << desiredBasePose, centroidal_model::getJointAngles(desiredState, info);
  vDesired << desiredBaseVelocity, centroidal_model::getJointVelocities(desiredInput, info);
  aDesired << desiredBaseAcceleration, desiredJointAccelerations;

  pinocchio::container::aligned_vector<pinocchio::Force> fextDesired(model.njoints, pinocchio::Force::Zero());
  for (size_t i = 0; i < info.numThreeDofContacts; i++) {
    const auto frameIndex = info.endEffectorFrameIndices[i];
    const auto jointIndex = model.frames[frameIndex].parent;
    const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation();
    const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose();
    const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info);
    fextDesired[jointIndex].linear() = contactForce;
    fextDesired[jointIndex].angular() = translationJointFrameToContactFrame.cross(contactForce);
  }
  for (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) {
    const auto frameIndex = info.endEffectorFrameIndices[i];
    const auto jointIndex = model.frames[frameIndex].parent;
    const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation();
    const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose();
    const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info);
    const Vector3 contactTorque = rotationWorldFrameToJointFrame * centroidal_model::getContactTorques(desiredInput, i, info);
    fextDesired[jointIndex].linear() = contactForce;
    fextDesired[jointIndex].angular() = translationJointFrameToContactFrame.cross(contactForce) + contactTorque;
  }

  // measured
  vector_t qMeasured(info.generalizedCoordinatesNum), vMeasured(info.generalizedCoordinatesNum);
  qMeasured.head<3>() = measuredRbdState.segment<3>(3);
  qMeasured.segment<3>(3) = measuredRbdState.head<3>();
  qMeasured.tail(info.actuatedDofNum) = measuredRbdState.segment(6, info.actuatedDofNum);
  vMeasured.head<3>() = measuredRbdState.segment<3>(info.generalizedCoordinatesNum + 3);
  vMeasured.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
      qMeasured.segment<3>(3), measuredRbdState.segment<3>(info.generalizedCoordinatesNum));
  vMeasured.tail(info.actuatedDofNum) = measuredRbdState.segment(info.generalizedCoordinatesNum + 6, info.actuatedDofNum);

  // PD feedback augmentation
  const vector_t pdFeedback = pGains.cwiseProduct(qDesired - qMeasured) + dGains.cwiseProduct(vDesired - vMeasured);

  // feedforward plus PD on acceleration level
  const vector_t aAugmented = aDesired + pdFeedback;
  return pinocchio::rnea(model, data, qDesired, vDesired, aAugmented, fextDesired);
}

The specific line of code in question is:

  // PD feedback augmentation
  const vector_t pdFeedback = pGains.cwiseProduct(qDesired - qMeasured) + dGains.cwiseProduct(vDesired - vMeasured);

In the calculation of pdFeedback, the velocity desired (vDesired) is represented using global angular velocity, whereas the velocity measured (vMeasured) is represented using ZYX Euler angle rates. This discrepancy in the representation leads to incorrect PD feedback.

Expected behavior
It appears that vDesired should be converted from global angular velocities to ZYX Euler angle rates using the function getEulerAnglesZyxDerivativesFromGlobalAngularVelocity for consistency.

  auto desiredZyxDerivatives = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(desiredBasePose.tail<3>(), desiredBaseVelocity);
  vDesired << desiredZyxDerivatives, centroidal_model::getJointVelocities(desiredInput, info);

however, it seems that there is no similar function available that can convert aDesired from global angular acceleration to the second derivative of ZYX Euler angles

aDesired << desiredBaseAcceleration, desiredJointAccelerations;