Allow malformed meshes for visualization purposes. Throw error only if the mesh is required for inertia computation.

PiperOrigin-RevId: 466421062
Change-Id: I947dbc107e481bd8c328b1a6430ab2e88bc82fd6

src/user/user_mesh.cc

@@ -81,6 +81,11 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   face = NULL;
   graph = NULL;
   needhull = false;
+  validorientation = true;
+  validarea = true;
+  validvolume = true;
+  valideigenvalue = true;
+  validinequality = true;
 
   // reset to default if given
   if (_def) {
@@ -223,8 +228,9 @@ void mjCMesh::Compile(const mjVFS* vfs) {
   if (!useredge.empty()) {
     std::sort(useredge.begin(), useredge.end());
     auto iterator = std::adjacent_find(useredge.begin(), useredge.end());
-    if (iterator != useredge.end())
+    if (iterator != useredge.end()) {
-      throw mjCError(this, "faces have inconsistent orientation");
+      validorientation = false;
+    }
   }
 
   // require vertices
@@ -248,7 +254,9 @@ void mjCMesh::Compile(const mjVFS* vfs) {
   }
 
   // scale, center, orient, compute mass and inertia
-  Process();
+  if (validorientation) {
+    Process();
+  }
 }
 
 
@@ -920,7 +928,7 @@ void mjCMesh::Process() {
           vert[3*i+1] = (float) p1[1];
           vert[3*i+2] = (float) p1[2];
 
-          // nromals
+          // normals
           mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
           mju_rotVecMatT(n1, n0, mat);
           normal[3*i] = (float) n1[0];
@@ -983,7 +991,8 @@ void mjCMesh::Process() {
 
       // require positive area
       if (area < mjMINVAL) {
-        throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
+        validarea = false;
+        return;
       }
 
       // finalize centroid of faces
@@ -1016,7 +1025,8 @@ void mjCMesh::Process() {
 
     // require positive volume
     if (GetVolumeRef(type) < mjMINVAL) {
-      throw mjCError(this, "mesh volume is too small: %s", name.c_str());
+      validvolume = false;
+      return;
     }
 
     // finalize CoM, save as mesh center
@@ -1085,13 +1095,14 @@ void mjCMesh::Process() {
 
     // check eigval - SHOULD NOT OCCUR
     if (eigval[2]<=0) {
-      throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
+      valideigenvalue = false;
+      return;
     }
     if (eigval[0] + eigval[1] < eigval[2] ||
         eigval[0] + eigval[2] < eigval[1] ||
         eigval[1] + eigval[2] < eigval[0]) {
-      throw mjCError(this,
+      validinequality = false;
-                    "eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str());
+      return;
     }
 
     // compute sizes of equivalent inertia box
@@ -1140,8 +1151,24 @@ void mjCMesh::Process() {
 }
 
 
+// check that the mesh is valid
+void mjCMesh::CheckMesh() {
+  if (!validorientation)
+    throw mjCError(this, "faces have inconsistent orientation: %s", name.c_str());
+  if (!validarea)
+    throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
+  if (!validvolume)
+    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());
+  if (!validinequality)
+    throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str());
+}
+
+
 // compute inertia
 double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
+  CheckMesh();
   if (type==mjSHELL_MESH) {
     return boxsz_surface;
   } else {
@@ -1151,6 +1178,7 @@ double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
 
 
 double& mjCMesh::GetVolumeRef(mjtMeshType type) {
+  CheckMesh();
   if (type) {
     return surface;
   } else {

src/user/user_objects.cc

@@ -295,7 +295,7 @@ mjCBody::mjCBody(mjCModel* _model) {
   pos[0] = ipos[0] = mjNAN;
 
   // clear variables
-  explicit_inertial = false;
+  explicitinertial = false;
   mocap = false;
   mjuu_setvec(quat, 1, 0, 0, 0);
   mjuu_setvec(iquat, 1, 0, 0, 0);
@@ -634,9 +634,9 @@ void mjCBody::MakeLocal(double* _locpos, double* _locquat,
   }
 }
 
-// set explicit_inertial to true
+// set explicitinertial to true
 void mjCBody::MakeInertialExplicit() {
-  explicit_inertial = true;
+  explicitinertial = true;
 }
 
 
@@ -680,6 +680,8 @@ void mjCBody::Compile(void) {
 
   // compile all geoms, phase 1
   for (i=0; i<geoms.size(); i++) {
+    geoms[i]->inferinertia = id>0 && (!explicitinertial ||
+                                      model->inertiafromgeom==mjINERTIAFROMGEOM_TRUE);
     geoms[i]->Compile();
   }
 
@@ -968,6 +970,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
   rgba[3] = 1.0f;
   userdata.clear();
   typeinertia = mjVOLUME_MESH;
+  inferinertia = true;
 
   // clear internal variables
   mjuu_setvec(quat, 1, 0, 0, 0);
@@ -1385,18 +1388,26 @@ void mjCGeom::Compile(void) {
   }
 
   // compute geom mass and inertia
-  if (mjuu_defined(_mass) && GetVolume()>mjMINVAL) {
+  if (inferinertia) {
-    mass = _mass;
+    if (mjuu_defined(_mass)) {
-    density = _mass / GetVolume();
+      if (_mass==0) {
-  } else {
+        mass = 0;
-    mass = density * GetVolume();
+        density = 0;
-  }
+      } else if (GetVolume()>mjMINVAL) {
-  SetInertia();
+        mass = _mass;
+        density = _mass / GetVolume();
+        SetInertia();
+      }
+    } else {
+      mass = density * GetVolume();
+      SetInertia();
+    }
 
-  // check for negative values
+    // check for negative values
-  if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
+    if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
-    throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)",
+      throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)",
-                   name.c_str(), id);
+                    name.c_str(), id);
+  }
 
   // fluid-interaction coefficients, requires computed inertia and mass
   if (fluid_switch > 0) {

src/user/user_objects.h

@@ -167,7 +167,7 @@ class mjCBody : public mjCBase {
   // setup child local frame, take into account change
   void MakeLocal(double* locpos, double* locquat, const double* pos, const double* quat);
 
-  // set explicit_inertial to true
+  // set explicitinertial to true
   void MakeInertialExplicit();
 
   // variables set by user or 'Compile'
@@ -198,7 +198,7 @@ class mjCBody : public mjCBase {
   int weldid;                     // top index of body we are welded to
   int dofnum;                     // number of motion dofs for body
   int mocapid;                    // mocap id, -1: not mocap
-  bool explicit_inertial;         // whether to save the body with an explicit inertial clause
+  bool explicitinertial;          // whether to save the body with an explicit inertial clause
 
   int lastdof;                    // id of last dof (used by compiler)
 
@@ -306,6 +306,7 @@ class mjCGeom : public mjCBase {
   std::vector<double> userdata;   // user data
   float rgba[4];                  // rgba when material is omitted
   mjtMeshType typeinertia;        // selects between surface and volume inertia
+  bool inferinertia;              // true if inertia has to be computed from geom
 
   // variables set by user and used during compilation
   double _mass;                   // used to compute density
@@ -481,7 +482,15 @@ class mjCMesh: public mjCBase {
   void MakeNormal(void);                      // compute vertex normals
   void Process();                             // apply transformations
   void RemoveRepeated(void);                  // remove repeated vertices
-  void ComputeInertia(mjtMeshType type);     // compute inertia
+  void ComputeInertia(mjtMeshType type);      // compute inertia
+  void CheckMesh(void);                       // check if the mesh is valid
+
+  // mesh properties that indicate a well-formed mesh
+  bool validorientation;              // false if mesh have inconsistent faces
+  bool validarea;                     // false if the area is too small
+  bool validvolume;                   // false if the volume is too small
+  bool valideigenvalue;               // false if inertia eigenvalue is too small
+  bool validinequality;               // false if inertia inequality is not satisfied
 
   // mesh properties computed by Compile
   double pos_volume[3];               // CoM position

src/xml/xml_native_reader.cc

@@ -2284,7 +2284,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
       if (pbody->id==0) {
         throw mjXError(elem, "World body cannot have inertia");
       }
-      pbody->explicit_inertial = true;
+      pbody->explicitinertial = true;
       ReadAttr(elem, "pos", 3, pbody->ipos, text, true);
       ReadAttr(elem, "quat", 4, pbody->iquat, text);
       ReadAttr(elem, "mass", 1, &pbody->mass, text, true);

src/xml/xml_native_writer.cc

@@ -1178,7 +1178,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
     WriteVector(elem, "user", body->userdata);
 
     // write inertial
-    if (body->explicit_inertial &&
+    if (body->explicitinertial &&
         model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) {
       XMLElement* inertial = InsertEnd(elem, "inertial");
       WriteAttr(inertial, "pos", 3, body->locipos);

src/xml/xml_urdf.cc

@@ -207,7 +207,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
 
   // inertial element: copy into alternative body frame
   if ((elem = FindSubElem(body_elem, "inertial"))) {
-    pbody->explicit_inertial = true;
+    pbody->explicitinertial = true;
     // origin- relative to joint frame for now
     Origin(elem, pbody->ipos, pbody->iquat);
 

test/user/testdata/malformed_face.xml

@@ -3,6 +3,8 @@
     <mesh file="malformed_face.obj"/>
   </asset>
   <worldbody>
-    <geom type="mesh" mesh="malformed_face"/>
+    <body>
+      <geom type="mesh" mesh="malformed_face"/>
+    </body>
   </worldbody>
 </mujoco>

test/user/user_mesh_test.cc

@@ -188,11 +188,32 @@ TEST_F(MujocoTest, MalformedFaceFails) {
   const std::string xml_path = GetTestDataFilePath(kMalformedFaceOBJPath);
   std::array<char, 1024> error;
   mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
-  ASSERT_THAT(model, testing::IsNull());
+  EXPECT_THAT(model, testing::IsNull());
   EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
 }
 
 TEST_F(MujocoTest, FlippedFaceFails) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1"
+        face="2 0 3  0 1 3  1 2 3  0 1 2" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
+}
+
+TEST_F(MujocoTest, FlippedFaceAllowedWorld) {
   static constexpr char xml[] = R"(
   <mujoco>
     <asset>
@@ -206,10 +227,136 @@ TEST_F(MujocoTest, FlippedFaceFails) {
   </mujoco>
   )";
   std::array<char, 1024> error;
-  LoadModelFromString(xml, error.data(), error.size());
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
+  EXPECT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
 }
 
+TEST_F(MujocoTest, FlippedFaceAllowedNoMass) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1"
+        face="2 0 3  0 1 3  1 2 3  0 1 2" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh" mass="0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(MujocoTest, FlippedFaceAllowedInertial) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1"
+        face="2 0 3  0 1 3  1 2 3  0 1 2" />
+    </asset>
+    <worldbody>
+      <body>
+        <inertial pos="0 0 0" mass="1"/>
+        <geom type="mesh" mesh="example_mesh"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(MujocoTest, AreaTooSmall) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1e-8 0 0  0 1e-8 0  0 0 1e-8"
+        face="2 0 3  0 1 3  1 2 3  0 2 1" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
+}
+
+TEST_F(MujocoTest, AreaTooSmallAllowedWorld) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1e-8 0 0  0 1e-8 0  0 0 1e-8"
+        face="2 0 3  0 1 3  1 2 3  0 2 1" />
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="example_mesh"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(MujocoTest, VolumeTooSmall) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1"
+        face="0 2 1" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
+}
+
+  TEST_F(MujocoTest, VolumeTooSmallAllowedWorld) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1"
+        face="0 2 1" />
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="example_mesh"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+// ------------- test concave and shell inertia --------------------------------
+
 const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
 
 TEST_F(MujocoTest, ExactConcaveInertia) {