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) {
-    // we ignore the first body as it is the world body
-    if (!CheckBodiesMassInertia(std::vector<mjCBody*>(bodies_.begin()+1, bodies_.end()))) {
-      throw mjCError(0, "mass and inertia of moving bodies must be larger than mjMINVAL");
+  for (int i=1; i < bodies_.size(); i++) {
+    if (!bodies_[i]->joints.empty() && !CheckBodyMassInertia(bodies_[i])) {
+      throw mjCError(bodies_[i], "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);