Use shell inertia to compute mesh aabb and frame if volume is invalid.

PiperOrigin-RevId: 530925161
Change-Id: I94b97efc7198e2b750480df8687e91bc047a6fc9

doc/changelog.rst

@@ -9,6 +9,14 @@ Upcoming version (not yet released)
    :align: right
    :width: 240px
 
+
+Bug fixes
+^^^^^^^
+
+- Fixed a bug that was causing an incorrect computation of the mesh bounding box and coordinate frame if the volume was
+  invalid. In such case, now MuJoCo only accepts a non-watertight geometry if :ref:`shellinertia<body-geom-shellinertia>`
+  is equal to ``true`.
+
 Plugins
 ^^^^^^^
 

src/user/user_mesh.cc

@@ -912,18 +912,7 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
 
 
 // apply transformations
-void mjCMesh::Process() {
+void mjCMesh::ApplyTransformations() {
-  for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
-    double CoM[3] = {0, 0, 0};
-    double facecen[3] = {0, 0, 0};
-    double area = 0;
-    double inert[6] = {0, 0, 0, 0, 0, 0};
-
-    double nrm[3];
-    double cen[3];
-    bool exactmeshinertia = model->exactmeshinertia;
-
-    if (type==mjVOLUME_MESH) {
   // translate
   if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
     // prepare translation
@@ -996,8 +985,15 @@ void mjCMesh::Process() {
       normal[3*i+2] = 1;
     }
   }
+}
+
 
 // find centroid of faces
+void mjCMesh::ComputeFaceCentroid(double facecen[3]) {
+  double area = 0;
+  double nrm[3];
+  double cen[3];
+
   for (int i=0; i<nface; i++) {
     // check vertex indices
     for (int j=0; j<3; j++) {
@@ -1028,21 +1024,38 @@ void mjCMesh::Process() {
   }
 }
 
+
+void mjCMesh::Process() {
+  double facecen[3] = {0, 0, 0};
+  double nrm[3];
+  double cen[3];
+
+  // user offset, rotation, scaling
+  ApplyTransformations();
+
+  // find centroid of faces
+  ComputeFaceCentroid(facecen);
+
+  // compute inertial properties for both inertia types
+  for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
+    double CoM[3] = {0, 0, 0};
+    double inert[6] = {0, 0, 0, 0, 0, 0};
+    bool exactmeshinertia = model->exactmeshinertia;
+
     // compute CoM and volume from pyramid volumes
     ComputeVolume(CoM, type, facecen, model->exactmeshinertia);
 
-    // perform computation again if volume is negative
+    // perform computation with convex mesh if volume is negative
-    if (GetVolumeRef(type) < mjMINVAL && exactmeshinertia) {
+    if (GetVolumeRef(type) <= 0 && exactmeshinertia) {
       mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str());
       exactmeshinertia = false;
       ComputeVolume(CoM, type, facecen, exactmeshinertia);
     }
 
-
+    // if volume is still invalid, skip the rest of the computations
-    // require positive volume
     if (GetVolumeRef(type) < mjMINVAL) {
       validvolume = GetVolumeRef(type) < 0 ? -1 : 0;
-      return;
+      continue;
     }
 
     // finalize CoM, save as mesh center
@@ -1052,7 +1065,7 @@ void mjCMesh::Process() {
     mjuu_copyvec(GetPosPtr(type), CoM, 3);
 
     // re-center mesh at CoM
-    if (type==mjVOLUME_MESH) {
+    if (type==mjVOLUME_MESH || validvolume<=0) {
       for (int i=0; i<nvert; i++) {
         for (int j=0; j<3; j++) {
           vert[3*i+j] -= CoM[j];
@@ -1128,13 +1141,15 @@ void mjCMesh::Process() {
     boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
     boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
 
-    // copy quat
+    // if volume was valid, copy volume quat to shell and stop,
-    for (int j=0; j<4; j++) {
+    // otherwise use shell quat for coordinate transformations
-      GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j];
+    if (type==mjSHELL_MESH && validvolume>0) {
+      mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH));
+      continue;
     }
 
     // rotate vertices and normals into axis-aligned frame
-    if (type==mjVOLUME_MESH) {
+    mju_copy4(GetQuatPtr(type), quattmp);
     double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
     double mat[9];
     mjuu_quat2mat(mat, neg);
@@ -1162,11 +1177,10 @@ void mjCMesh::Process() {
     }
   }
 }
-}
 
 
 // check that the mesh is valid
-void mjCMesh::CheckMesh() {
+void mjCMesh::CheckMesh(mjtMeshType type) {
   if (!processed) {
     return;
   }
@@ -1175,11 +1189,11 @@ void mjCMesh::CheckMesh() {
                    "faces of mesh '%s' have inconsistent orientation. Please check the "
                    "faces containing the vertices %d and %d.",
                    name.c_str(), invalidorientation.first, invalidorientation.second);
-  if (!validarea)
+  if (!validarea && type==mjSHELL_MESH)
     throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
-  if (validvolume<0)
+  if (validvolume<0 && type==mjVOLUME_MESH)
     throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
-  if (!validvolume)
+  if (!validvolume && type==mjVOLUME_MESH)
     throw mjCError(this, "mesh volume is too small: %s", name.c_str());
   if (!valideigenvalue)
     throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
@@ -1190,13 +1204,13 @@ void mjCMesh::CheckMesh() {
 
 // get inertia pointer
 double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
-  CheckMesh();
+  CheckMesh(type);
   return type==mjSHELL_MESH ? boxsz_surface : boxsz_volume;
 }
 
 
 double& mjCMesh::GetVolumeRef(mjtMeshType type) {
-  CheckMesh();
+  CheckMesh(type);
   return type==mjSHELL_MESH ? surface : volume;
 }
 

src/user/user_objects.h

@@ -554,9 +554,11 @@ class mjCMesh: public mjCBase {
   void CopyGraph(void);                       // copy graph into face data
   void MakeNormal(void);                      // compute vertex normals
   void MakeCenter(void);                      // compute face circumcircle data
-  void Process();                             // apply transformations
+  void Process();                             // compute inertial properties
+  void ApplyTransformations();                // apply user transformations
+  void ComputeFaceCentroid(double[3]);        // compute centroid of all faces
   void RemoveRepeated(void);                  // remove repeated vertices
-  void CheckMesh(void);                       // check if the mesh is valid
+  void CheckMesh(mjtMeshType type);           // check if the mesh is valid
 
   // compute the volume and center-of-mass of the mesh given the face center
   void ComputeVolume(double CoM[3], mjtMeshType type, const double facecen[3],

test/user/user_mesh_test.cc

@@ -493,7 +493,7 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
     </asset>
     <worldbody>
       <body>
-        <geom type="mesh" mesh="example_mesh"/>
+        <geom type="mesh" mesh="example_mesh" shellinertia="true"/>
       </body>
     </worldbody>
   </mujoco>
@@ -544,6 +544,31 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
   EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
 }
 
+TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <compiler convexhull="false"/>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  1 1 0"
+        face="0 1 2  2 1 3" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh" shellinertia="true"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, testing::NotNull());
+  EXPECT_LE(mju_abs(model->geom_size[0]), 1);
+  EXPECT_LE(mju_abs(model->geom_size[1]), 1);
+  EXPECT_LE(mju_abs(model->geom_size[2]), 1);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjCMeshTest, VolumeNegativeDefaultsLegacy) {
   static constexpr char xml[] = R"(
   <mujoco>