From cfd866a54cab84505fb9e04f0b83d42240ee1952 Mon Sep 17 00:00:00 2001 From: Renkunzhao <3281634601@qq.com> Date: Tue, 3 Sep 2024 21:12:02 +0800 Subject: [PATCH 1/2] bug fixed for RaisimHeightmapRosConverter --- .../src/RaisimHeightmapRosConverter.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp index 04bbee0b8..f4a94fd41 100644 --- a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp +++ b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp @@ -59,10 +59,10 @@ grid_map_msgs::GridMapPtr RaisimHeightmapRosConverter::convertHeightmapToGridmap dataArray.layout.dim.resize(2); dataArray.layout.dim[0].label = "column_index"; dataArray.layout.dim[0].stride = heightMap.getHeightVector().size(); - dataArray.layout.dim[0].size = heightMap.getXSamples(); + dataArray.layout.dim[0].size = heightMap.getYSamples(); dataArray.layout.dim[1].label = "row_index"; - dataArray.layout.dim[1].stride = heightMap.getYSamples(); - dataArray.layout.dim[1].size = heightMap.getYSamples(); + dataArray.layout.dim[1].stride = heightMap.getXSamples(); + dataArray.layout.dim[1].size = heightMap.getXSamples(); dataArray.data.insert(dataArray.data.begin(), heightMap.getHeightVector().rbegin(), heightMap.getHeightVector().rend()); gridMapMsg->data.push_back(dataArray); @@ -74,8 +74,8 @@ std::unique_ptr RaisimHeightmapRosConverter::convertGridmapTo throw std::runtime_error("RaisimHeightmapRosConverter::convertGridmapToHeightmap - Layout of gridMap currently not supported"); } - const int xSamples = gridMap->data[0].layout.dim[0].size; - const int ySamples = gridMap->data[0].layout.dim[1].size; + const int xSamples = gridMap->data[0].layout.dim[1].size; + const int ySamples = gridMap->data[0].layout.dim[0].size; const double xSize = gridMap->info.length_x; const double ySize = gridMap->info.length_y; const double centerX = gridMap->info.pose.position.x; From 9cfad18848f24c138cb884aeecbb52add604e97e Mon Sep 17 00:00:00 2001 From: Renkunzhao <3281634601@qq.com> Date: Wed, 4 Sep 2024 10:30:01 +0800 Subject: [PATCH 2/2] bug fixed for computeRbdTorqueFromCentroidalModelPD --- .../src/CentroidalModelRbdConversions.cpp | 8 +++-- .../common/RotationDerivativesTransforms.h | 36 +++++++++++++++++++ 2 files changed, 42 insertions(+), 2 deletions(-) diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 2d99c9789..143f759a2 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -183,8 +183,12 @@ vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(co 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; + // vDesired << desiredBaseVelocity, centroidal_model::getJointVelocities(desiredInput, info); + // aDesired << desiredBaseAcceleration, desiredJointAccelerations; + Vector3 desiredZyxDerivatives = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity(desiredBasePose.tail<3>(), desiredBaseVelocity.tail<3>()); + vDesired << desiredBaseVelocity.head<3>(), desiredZyxDerivatives, centroidal_model::getJointVelocities(desiredInput, info); + Vector3 desiredZyxSencondDerivatives = getEulerAnglesZyxDerivativesFromGlobalAngularAcceleration(desiredBasePose.tail<3>(), desiredZyxDerivatives, desiredBaseAcceleration.tail<3>()); + aDesired << desiredBaseAcceleration.head<3>(), desiredZyxSencondDerivatives , desiredJointAccelerations; // TODO desiredBaseAcceleration should be transfer to secondDerivativesEulerAngles pinocchio::container::aligned_vector fextDesired(model.njoints, pinocchio::Force::Zero()); for (size_t i = 0; i < info.numThreeDofContacts; i++) { diff --git a/ocs2_robotic_tools/include/ocs2_robotic_tools/common/RotationDerivativesTransforms.h b/ocs2_robotic_tools/include/ocs2_robotic_tools/common/RotationDerivativesTransforms.h index 2d5846398..aef34a172 100644 --- a/ocs2_robotic_tools/include/ocs2_robotic_tools/common/RotationDerivativesTransforms.h +++ b/ocs2_robotic_tools/include/ocs2_robotic_tools/common/RotationDerivativesTransforms.h @@ -132,6 +132,42 @@ Eigen::Matrix getGlobalAngularAccelerationFromEulerAnglesZyxDeri return derivativeTransformationMatrix * derivativesEulerAngles + transformationMatrix * secondDerivativesEulerAngles; } +/** + * Compute derivatives of ZYX-Euler angles from angular accelerations expressed in the world frame + * + * @param [in] eulerAngles: ZYX-Euler angles + * @param [in] derivativesEulerAngles: time-derivative of ZYX-Euler angles + * @param [in] angularAcceleration: angular acceleration expressed in world frame + * @return second order time-derivative of ZYX-Euler angles + */ +template +Eigen::Matrix getEulerAnglesZyxDerivativesFromGlobalAngularAcceleration( + const Eigen::Matrix& eulerAngles, const Eigen::Matrix& derivativesEulerAngles, + const Eigen::Matrix& angularAcceleration) { + const SCALAR_T sz = sin(eulerAngles(0)); + const SCALAR_T cz = cos(eulerAngles(0)); + const SCALAR_T sy = sin(eulerAngles(1)); + const SCALAR_T cy = cos(eulerAngles(1)); + const SCALAR_T sx = sin(eulerAngles(2)); + const SCALAR_T cx = cos(eulerAngles(2)); + const SCALAR_T dz = derivativesEulerAngles(0); + const SCALAR_T dy = derivativesEulerAngles(1); + const SCALAR_T dx = derivativesEulerAngles(2); + Eigen::Matrix transformationMatrixInv; + // clang-format off + transformationMatrixInv << cz*sy/cy, sz*sy/cy, SCALAR_T(1.0), + -sz, cz, SCALAR_T(0.0), + cz/cy, sz/cy, SCALAR_T(0.0); + // clang-format on + Eigen::Matrix derivativeTransformationMatrix; + // clang-format off + derivativeTransformationMatrix << SCALAR_T(0), -cz*dz, (-sy*dy)*cz + cy*(-sz*dz), + SCALAR_T(0), -sz*dz, (-sy*dy)*sz + cy*(cz*dz), + SCALAR_T(0), SCALAR_T(0), -cy*dy; + // clang-format on + return transformationMatrixInv * (angularAcceleration - derivativeTransformationMatrix * derivativesEulerAngles); +} + /** * Compute the matrix that maps derivatives of ZYX-Euler angles to local angular velocities *