Describe the bug
I have identified an issue within the computeRbdTorqueFromCentroidalModelPD function, where the Proportional-Derivative (PD) feedback calculation appears to be incorrectly implemented.
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
Describe the bug I have identified an issue within the computeRbdTorqueFromCentroidalModelPD function, where the Proportional-Derivative (PD) feedback calculation appears to be incorrectly implemented.
The specific line of code in question is:
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.
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