diff --git a/genesis/engine/solvers/rigid/rigid_solver.py b/genesis/engine/solvers/rigid/rigid_solver.py index 8b3ede2ba0..03cd248b1e 100644 --- a/genesis/engine/solvers/rigid/rigid_solver.py +++ b/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: diff --git a/pyproject.toml b/pyproject.toml index 41d72ccf59..1a65a59299 100644 --- a/pyproject.toml +++ b/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", diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index 30d130971a..6c361f4be2 100644 --- a/tests/test_rigid_physics.py +++ b/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, diff --git a/tests/utils.py b/tests/utils.py index 02c863fee6..25b9f913b8 100644 --- a/tests/utils.py +++ b/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)