Report which body has too-small mass/inertia in compilation error.

PiperOrigin-RevId: 776196017
Change-Id: Ib81efb170347a57cdc4b446c14cd7b8f5357958c

src/user/user_model.cc

@@ -4515,10 +4515,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
   }
 
   // check mass and inertia of moving bodies
-  if (bodies_.size() > 1) {
+  for (int i=1; i < bodies_.size(); i++) {
-    // we ignore the first body as it is the world body
+    if (!bodies_[i]->joints.empty() && !CheckBodyMassInertia(bodies_[i])) {
-    if (!CheckBodiesMassInertia(std::vector<mjCBody*>(bodies_.begin()+1, bodies_.end()))) {
+      throw mjCError(bodies_[i], "mass and inertia of moving bodies must be larger than mjMINVAL");
-      throw mjCError(0, "mass and inertia of moving bodies must be larger than mjMINVAL");
     }
   }
 
@@ -4763,20 +4762,6 @@ uint64_t mjCModel::Signature() {
 
 
 
-bool mjCModel::CheckBodiesMassInertia(std::vector<mjCBody*> bodies) {
-  // check mass and inertia of moving bodies
-  for (int i=0; i < bodies.size(); i++) {
-    if (!bodies[i]->joints.empty()) {
-      if (!CheckBodyMassInertia(bodies[i])) {
-        return false;
-      }
-    }
-  }
-  return true;
-}
-
-
-
 bool mjCModel::CheckBodyMassInertia(mjCBody* body) {
   // check if body has valid mass and inertia
   if (body->mass >= mjMINVAL &&

src/user/user_model.h

@@ -425,9 +425,6 @@ class mjCModel : public mjCModel_, private mjSpec {
   // compute qpos0
   void ComputeReference();
 
-  // return true if all bodies have valid mass and inertia
-  bool CheckBodiesMassInertia(std::vector<mjCBody*> bodies);
-
   // return true if body has valid mass and inertia
   bool CheckBodyMassInertia(mjCBody* body);
 

test/user/user_mesh_test.cc

@@ -529,7 +529,7 @@ TEST_F(MjCMeshTest, TinyInertiaFails) {
       <mesh name="tiny" vertex="0 0 0  1e-4 0 0  0 1e-4 0  0 0 1e-4"/>
     </asset>
     <worldbody>
-      <body>
+      <body name="tiny_body">
         <freejoint/>
         <geom type="mesh" mesh="tiny"/>
       </body>
@@ -542,6 +542,7 @@ TEST_F(MjCMeshTest, TinyInertiaFails) {
       error.data(),
       HasSubstr(
           "mass and inertia of moving bodies must be larger than mjMINVAL"));
+  EXPECT_THAT(error.data(), HasSubstr("Element name 'tiny_body'"));
 }
 
 TEST_F(MjCMeshTest, FlippedFaceAllowedLegacyInertia) {

test/user/user_model_test.cc

@@ -196,7 +196,7 @@ TEST_F(UserCModelTest, NestedZeroMassBodiesFail) {
     <worldbody>
       <body>
         <geom size="1"/>
-        <body>
+        <body name="bad">
           <freejoint/>
           <body>
             <body>
@@ -214,6 +214,7 @@ TEST_F(UserCModelTest, NestedZeroMassBodiesFail) {
       error,
       HasSubstr(
           "mass and inertia of moving bodies must be larger than mjMINVAL"));
+  EXPECT_THAT(error, HasSubstr("Element name 'bad'"));
   mj_deleteModel(model);
 }