From f695f552538391a9bd0d3ade06d44cad5d787913 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Wed, 26 Oct 2022 09:44:44 -0700
Subject: [PATCH] Do not infer inertia if geom is not in inertiagrouprange.

PiperOrigin-RevId: 483994271
Change-Id: Ia663cd30e93d9c3a2e32408a658f091af4d1ca1a
---
 src/user/user_objects.cc       |  6 ++++--
 test/user/user_objects_test.cc | 38 ++++++++++++++++++++++++++++++++++
 2 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index 67546e13..22313ec3 100644
--- a/src/user/user_objects.cc
+++ b/src/user/user_objects.cc
@@ -701,8 +701,10 @@ 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]->inferinertia = id>0 &&
+      (!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
+      geoms[i]->group >= model->inertiagrouprange[0] &&
+      geoms[i]->group <= model->inertiagrouprange[1];
     geoms[i]->Compile();
   }
 
diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc
index 5acbdfc5..357c1dda 100644
--- a/test/user/user_objects_test.cc
+++ b/test/user/user_objects_test.cc
@@ -265,6 +265,44 @@ TEST_F(MjCGeomTest, CapsuleInertiaX) {
   mj_deleteModel(model);
 }
 
+// ------------- test inertiagrouprange ----------------------------------------
+TEST_F(MjCGeomTest, IgnoreGeomOutsideInertiagrouprange) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <compiler inertiagrouprange="0 1"/>
+    <worldbody>
+      <body>
+        <geom size="1" group="3"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* m = LoadModelFromString(xml, nullptr, 0);
+  EXPECT_THAT(m->body_mass[1], 0);
+  mj_deleteModel(m);
+}
+
+TEST_F(MjCGeomTest, IgnoreBadGeomOutsideInertiagrouprange) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <compiler inertiagrouprange="0 1"/>
+    <asset>
+      <mesh name="malformed_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="malformed_mesh" group="3"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* m = LoadModelFromString(xml, nullptr, 0);
+  EXPECT_THAT(m->body_mass[1], 0);
+  mj_deleteModel(m);
+}
+
 // ------------- test height fields --------------------------------------------
 
 using MjCHFieldTest = MujocoTest;