[BUG FIX] Fix degenerated invweight computation.

genesis/engine/solvers/rigid/rigid_solver.py

@@ -549,8 +549,18 @@ def _init_invweight_and_meaninertia(self, envs_idx=None, *, force_update=True):
                     A = jac @ mass_mat_inv @ jac.T
                     A_diag = np.diag(A)
 
-                    links_invweight[i_b_, i_l, 0] = A_diag[:3].mean()
-                    links_invweight[i_b_, i_l, 1] = A_diag[3:].mean()
+                    tran = A_diag[:3].mean()
+                    rot = A_diag[3:].mean()
+
+                    # If one component is zero, use the other to prevent degenerate constraints.
+                    # See https://github.com/google-deepmind/mujoco/commit/1cda1e7a
+                    if tran < gs.EPS and rot > gs.EPS:
+                        tran = rot
+                    elif rot < gs.EPS and tran > gs.EPS:
+                        rot = tran
+
+                    links_invweight[i_b_, i_l, 0] = tran
+                    links_invweight[i_b_, i_l, 1] = rot
 
             # Compute dofs invweight
             if i_b_ == 0 or self._options.batch_dofs_info:

pyproject.toml

@@ -86,7 +86,8 @@ dev = [
     "ipython",
     # * Mujoco 3.3.6 made contact islands an opt-out option instead of opt-in,
     #   which requires setting the disableflags 'mjDSBL_ISLAND'.
-    "mujoco>=3.3.6,<3.4.0",
+    # * Mujoco 3.5.0 fixed computation of link invweight when degenerated.
+    "mujoco>=3.5.0,<3.7.0",
     # Used internally by 'MPLPlotter'
     # * 3.7.0: introduces Button blitting and 'outside' keyword legend location
     "matplotlib>=3.7.0",

tests/test_rigid_physics.py

@@ -23,8 +23,6 @@
 from .utils import (
     assert_allclose,
     assert_equal,
-    build_genesis_sim,
-    build_mujoco_sim,
     check_mujoco_data_consistency,
     check_mujoco_model_consistency,
     get_hf_dataset,

tests/utils.py

@@ -18,6 +18,7 @@
 
 import cpuinfo
 import mujoco
+import pytest
 import numpy as np
 import torch
 from httpcore import TimeoutException as HTTPTimeoutException
@@ -701,7 +702,15 @@ def check_mujoco_model_consistency(
     for gs_i, mj_i in zip(gs_bodies_idx, mj_bodies_idx):
         gs_invweight_i = gs_sim.rigid_solver.links_info.invweight.to_numpy()[gs_i]
         mj_invweight_i = mj_sim.model.body(mj_i).invweight0
-        assert_allclose(gs_invweight_i, mj_invweight_i, tol=tol)
+        try:
+            assert_allclose(gs_invweight_i, mj_invweight_i, tol=tol)
+        except AssertionError:
+            if tuple(int(x) for x in mujoco.__version__.split(".")[:2]) < (3, 5):
+                pytest.skip(
+                    "MuJoCo < 3.5 lacks the degenerate invweight fix. "
+                    "See https://github.com/google-deepmind/mujoco/commit/1cda1e7a"
+                )
+            raise
         gs_inertia_i = gs_sim.rigid_solver.links_info.inertial_i.to_numpy()[gs_i, [0, 1, 2], [0, 1, 2]]
         mj_inertia_i = mj_sim.model.body(mj_i).inertia
         assert_allclose(gs_inertia_i, mj_inertia_i, tol=tol)