Ignore geoms with tiny mass in inertiafromgeom computation. Fixes #2754

PiperOrigin-RevId: 795475978
Change-Id: I02b5db9e2ac7ec556cb0a0ec6dbe4214ba9128d7

src/user/user_objects.cc

@@ -1902,11 +1902,12 @@ void mjCBody::InertiaFromGeom(void) {
   double toti[6] = {0, 0, 0, 0, 0, 0};
   std::vector<mjCGeom*> sel;
 
-  // select geoms based on group
+  // select geoms based on group, ignore tiny masses
   sel.clear();
   for (int i=0; i < geoms.size(); i++) {
     if (geoms[i]->group >= compiler->inertiagrouprange[0] &&
-        geoms[i]->group <= compiler->inertiagrouprange[1]) {
+        geoms[i]->group <= compiler->inertiagrouprange[1] &&
+        geoms[i]->mass_ > mjEPS) {
       sel.push_back(geoms[i]);
     }
   }

test/user/user_objects_test.cc

@@ -68,8 +68,7 @@ TEST_F(VfsTest, HFieldPngWithVFS) {
   // should fallback to OS filesystem
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -96,8 +95,7 @@ TEST_F(VfsTest, HFieldCustomWithVFS) {
   // should fallback to OS filesystem
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -125,8 +123,7 @@ TEST_F(VfsTest, TexturePngWithVFS) {
   // should fallback to OS filesystem
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -154,8 +151,7 @@ TEST_F(VfsTest, TextureCustomWithVFS) {
   // should fallback to OS filesystem
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -187,8 +183,7 @@ TEST_F(ContentTypeTest, HFieldPngWithContentType) {
   // should try loading the file
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -216,8 +211,7 @@ TEST_F(ContentTypeTest, HFieldCustomWithContentType) {
   // should try loading the file
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -273,8 +267,7 @@ TEST_F(ContentTypeTest, TexturePngWithContentType) {
   // should try loading the file
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -303,8 +296,7 @@ TEST_F(ContentTypeTest, TextureCustomWithContentType) {
   // should try loading the file
   mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error,
+  EXPECT_THAT(error, HasSubstr("Error opening file"));
-              HasSubstr("Error opening file"));
   mj_deleteVFS(vfs.get());
 }
 
@@ -369,7 +361,6 @@ TEST_F(ContentTypeTest, TextureLoadPng) {
   char error[1024];
   size_t error_sz = 1024;
 
-
   // load VFS on the heap
   auto vfs = std::make_unique<mjVFS>();
   mj_defaultVFS(vfs.get());
@@ -607,9 +598,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) {
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
   ASSERT_THAT(model, IsNull());
-  EXPECT_THAT(
+  EXPECT_THAT(error.data(), HasSubstr("unrecognized name 'alessio' of object"));
-      error.data(),
-      HasSubstr("unrecognized name 'alessio' of object"));
   EXPECT_THAT(error.data(), HasSubstr("name 'tom'"));
   EXPECT_THAT(error.data(), HasSubstr("line 7"));
 }
@@ -1199,7 +1188,8 @@ TEST_F(MjCJointTest, AlignFree) {
 
   // expect x-frames (sensors) to match to very high precision
   double eps = 1e-10;
-  EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
+  EXPECT_THAT(
+      AsVector(d->sensordata, m->nsensordata),
       Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
 
   // no frame sensors for lights, test separately
@@ -1224,10 +1214,10 @@ TEST_F(MjCJointTest, AlignFree) {
   EXPECT_GT(mju_norm(dqpos, nq), 1.0);
   mj_freeStack(d);
 
-
   // expect x-frames to match to reasonable precision
   eps = 1e-5;
-  EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
+  EXPECT_THAT(
+      AsVector(d->sensordata, m->nsensordata),
       Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
   EXPECT_THAT(AsVector(d->light_xpos, 3),
               Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
@@ -1247,8 +1237,7 @@ TEST_F(MjCJointTest, AlignFree) {
 using MjCHFieldTest = MujocoTest;
 
 TEST_F(MjCHFieldTest, PngMap) {
-  const string xml_path =
+  const string xml_path = GetTestDataFilePath("user/testdata/hfield_png.xml");
-      GetTestDataFilePath("user/testdata/hfield_png.xml");
   std::array<char, 1024> error;
   mjModel* model =
       mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
@@ -2402,7 +2391,8 @@ TEST_F(UserObjectsTest, BadConnect) {
   EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
   mj_deleteModel(m);
 
-  char error_missing[] = "either both body1 and anchor must be defined,"
+  char error_missing[] =
+      "either both body1 and anchor must be defined,"
       " or both site1 and site2 must be defined\nElement 'connect', line 12";
 
   // bad model (missing anchor)
@@ -2411,7 +2401,8 @@ TEST_F(UserObjectsTest, BadConnect) {
   ASSERT_THAT(m, IsNull());
   EXPECT_THAT(error, HasSubstr(error_missing));
 
-  char error_mixed[] = "body and site semantics cannot be mixed"
+  char error_mixed[] =
+      "body and site semantics cannot be mixed"
       "\nElement 'connect', line 12";
 
   // bad model (mixing body and site)
@@ -2611,5 +2602,25 @@ TEST_F(UserObjectsTest, Inertial) {
   EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot"));
 }
 
+TEST_F(UserObjectsTest, ZeroMass) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <body>
+          <geom mass="0" size="0.01"/>
+          <geom mass="0" size="0.01"/>
+        </body>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  EXPECT_THAT(model, NotNull()) << error;
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco