bug fixed for RaisimHeightmapRosConverter

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<scalar_t>(desiredBasePose.tail<3>(), desiredBaseVelocity.tail<3>());
+  vDesired << desiredBaseVelocity.head<3>(), desiredZyxDerivatives, centroidal_model::getJointVelocities(desiredInput, info);
+  Vector3 desiredZyxSencondDerivatives = getEulerAnglesZyxDerivativesFromGlobalAngularAcceleration<scalar_t>(desiredBasePose.tail<3>(), desiredZyxDerivatives, desiredBaseAcceleration.tail<3>());
+  aDesired << desiredBaseAcceleration.head<3>(), desiredZyxSencondDerivatives , desiredJointAccelerations;  // TODO desiredBaseAcceleration should be transfer to secondDerivativesEulerAngles
 
   pinocchio::container::aligned_vector<pinocchio::Force> fextDesired(model.njoints, pinocchio::Force::Zero());
   for (size_t i = 0; i < info.numThreeDofContacts; i++) {

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<raisim::HeightMap> 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;

ocs2_robotic_tools/include/ocs2_robotic_tools/common/RotationDerivativesTransforms.h

@@ -132,6 +132,42 @@ Eigen::Matrix<SCALAR_T, 3, 1> 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 <typename SCALAR_T>
+Eigen::Matrix<SCALAR_T, 3, 1> getEulerAnglesZyxDerivativesFromGlobalAngularAcceleration(
+    const Eigen::Matrix<SCALAR_T, 3, 1>& eulerAngles, const Eigen::Matrix<SCALAR_T, 3, 1>& derivativesEulerAngles,
+    const Eigen::Matrix<SCALAR_T, 3, 1>& 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<SCALAR_T, 3, 3> 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<SCALAR_T, 3, 3> 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
  *