From 981fa22302fd1686db4ca50ac6e370a0b000fa86 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Sat, 28 May 2022 06:42:48 -0700
Subject: [PATCH] Don't enforce minimum inertia at mesh loading.

Inertias of moving bodies are enforced to be greater mjMINVAL at model compilation, there is no need to enforce inertias at mesh load time. Tiny meshes could be static or part of a larger geom assembly, in which case their tiny inertia is not a problem.

Also, make error message for small masses/inertias more precise.

PiperOrigin-RevId: 451625216
Change-Id: I4b48d7a881e620d6fbb591ec77fbdacc7c03496c
---
 src/user/user_mesh.cc       |  4 +--
 src/user/user_model.cc      |  2 +-
 test/user/user_mesh_test.cc | 62 +++++++++++++++++++++++++++++++++++--
 3 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc
index e0d6f938..8d99c79f 100644
--- a/src/user/user_mesh.cc
+++ b/src/user/user_mesh.cc
@@ -1014,8 +1014,8 @@ void mjCMesh::Process(void) {
   mju_eig3(eigval, eigvec, quattmp, full);
 
   // check eigval - SHOULD NOT OCCUR
-  if (eigval[2]<mjEPS) {
-    throw mjCError(this, "eigenvalue of mesh inertia too small: %s", name.c_str());
+  if (eigval[2]<=0) {
+    throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
   }
   if (eigval[0] + eigval[1] < eigval[2] ||
       eigval[0] + eigval[2] < eigval[1] ||
diff --git a/src/user/user_model.cc b/src/user/user_model.cc
index 03f544b1..d43a4e06 100644
--- a/src/user/user_model.cc
+++ b/src/user/user_model.cc
@@ -2494,7 +2494,7 @@ mjModel* mjCModel::Compile(const mjVFS* vfs) {
 
         // error
         if (!ok) {
-          throw mjCError(b, "mass and inertia of moving bodies must be positive");
+          throw mjCError(b, "mass and inertia of moving bodies must be larger than mjMINVAL");
         }
       }
     }
diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc
index af5c2c5a..41435841 100644
--- a/test/user/user_mesh_test.cc
+++ b/test/user/user_mesh_test.cc
@@ -31,8 +31,6 @@ namespace {
 
 using MjCMeshTest = MujocoTest;
 
-// yuval: why do we want these anyway? why not string literals in the tests?
-// this does not improve readabillity IMO.
 static const char* const kDuplicateVerticesPath =
     "user/testdata/duplicate_vertices.xml";
 static const char* const kCubePath =
@@ -46,6 +44,8 @@ static const char* const kTexturedTorusPath =
 static const char* const kDuplicateOBJPath =
     "user/testdata/duplicate.xml";
 
+using ::testing::HasSubstr;
+
 // ------------- test vertex de-duplication (STL) ------------------------------
 
 TEST_F(MjCMeshTest, DeDuplicateSTLVertices) {
@@ -116,5 +116,63 @@ TEST_F(MjCMeshTest, SaveMeshOnce) {
   mj_deleteModel(model);
 }
 
+TEST_F(MujocoTest, TinyMeshLoads) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="tiny" vertex="0 0 0  1e-4 0 0  0 1e-4 0  0 0 1e-4"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="tiny"/>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml, 0, 0);
+  ASSERT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(MujocoTest, SmallInertiaLoads) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="tiny" vertex="0 0 0  1e-4 0 0  0 1e-4 0  0 0 1e-4"/>
+    </asset>
+    <worldbody>
+      <body>
+        <freejoint/>
+        <geom type="mesh" mesh="tiny"/>
+        <geom name="small" size=".001"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml, 0, 0);
+  ASSERT_THAT(model, testing::NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(MujocoTest, TinyInertiaFails) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="tiny" vertex="0 0 0  1e-4 0 0  0 1e-4 0  0 0 1e-4"/>
+    </asset>
+    <worldbody>
+      <body>
+        <freejoint/>
+        <geom type="mesh" mesh="tiny"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(
+      error.data(),
+      HasSubstr(
+          "mass and inertia of moving bodies must be larger than mjMINVAL"));
+}
+
 }  // namespace
 }  // namespace mujoco