Prevent degenerate body inverse weights in constraints.

Fixes #2472

PiperOrigin-RevId: 855223971
Change-Id: I646207f82353b7099aab9e220888fe11a4cd3d6d

src/engine/engine_setconst.c

@@ -376,8 +376,18 @@ static void set0(mjModel* m, mjData* d) {
       }
 
       // average diagonal and assign
-      m->body_invweight0[2*i] = (A[0] + A[7] + A[14])/3;
+      mjtNum tran = (A[0] + A[7] + A[14])/3;
-      m->body_invweight0[2*i+1] = (A[21] + A[28] + A[35])/3;
+      mjtNum rot = (A[21] + A[28] + A[35])/3;
+
+      // if one is zero, use the other to prevent degenerate constraints
+      if (tran < mjMINVAL && rot > mjMINVAL) {
+        tran = rot;  // use rotation as fallback for translation
+      } else if (rot < mjMINVAL && tran > mjMINVAL) {
+        rot = tran;  // use translation as fallback for rotation
+      }
+
+      m->body_invweight0[2*i] = tran;
+      m->body_invweight0[2*i+1] = rot;
     }
   }
 

test/engine/engine_forward_test.cc

@@ -810,6 +810,43 @@ TEST_F(ForwardTest, MocapQuats) {
   mj_deleteModel(model);
 }
 
+// model with degenerate translational inertia
+TEST_F(ForwardTest, DegenerateInertia) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option integrator="implicitfast" cone="elliptic">
+      <flag gravity="disable"/>
+    </option>
+    <worldbody>
+      <body name="1" pos="0.05 0.3 0">
+        <joint name="1" axis="0 1 0"/>
+        <geom type="capsule" size="0.1 0.5"/>
+      </body>
+      <body name="2">
+        <joint name="2" axis="1 0 0" stiffness="1" springref="90"/>
+        <geom type="capsule" size="0.1 0.5"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+  mjData* data = mj_makeData(model);
+
+  for (int i = 0; i < 1000; i++) {
+    mj_step(model, data);
+    EXPECT_EQ(data->warning[mjWARN_BADQACC].number, 0)
+        << "divergence at timestep " << i;
+    if (data->warning[mjWARN_BADQACC].number != 0) {
+      break;
+    }
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 // user defined 2nd-order activation dynamics: frequency-controlled oscillator
 //  note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but
 //  since we have a vector output we write into act_dot directly