diff --git a/src/openarm_control/qp/arm_joint_limit.py b/src/openarm_control/qp/arm_joint_limit.py index d25e2805..447c3f62 100644 --- a/src/openarm_control/qp/arm_joint_limit.py +++ b/src/openarm_control/qp/arm_joint_limit.py @@ -23,6 +23,13 @@ import numpy as np import numpy.typing as npt +# MuJoCo 3.12 enums no longer compare equal to NumPy scalars, and tuple +# membership compares element to needle, so keep joint-type checks int-to-int. +SCALAR_JOINT_TYPES = ( + int(mujoco.mjtJoint.mjJNT_HINGE), + int(mujoco.mjtJoint.mjJNT_SLIDE), +) + class ArmConfigurationLimit(mink.ConfigurationLimit): """Apply Mink's configuration limits only to selected scalar arm joints.""" @@ -44,10 +51,7 @@ def __init__( or int(model.jnt_qposadr[joint_id]) not in selected_qpos ): continue - if model.jnt_type[joint_id] not in ( - mujoco.mjtJoint.mjJNT_HINGE, - mujoco.mjtJoint.mjJNT_SLIDE, - ): + if int(model.jnt_type[joint_id]) not in SCALAR_JOINT_TYPES: raise ValueError("ArmConfigurationLimit only supports scalar joints.") active_dofs.append(int(model.jnt_dofadr[joint_id])) self.indices = _readonly(active_dofs, dtype=int) @@ -106,10 +110,7 @@ def __init__( "its velocity limit will be skipped." ) continue - if model.jnt_type[joint_id] not in ( - mujoco.mjtJoint.mjJNT_HINGE, - mujoco.mjtJoint.mjJNT_SLIDE, - ): + if int(model.jnt_type[joint_id]) not in SCALAR_JOINT_TYPES: raise ValueError("ArmJointLimit only supports scalar joints.") if name is None or name not in velocities: diff --git a/tests/test_qp_limits.py b/tests/test_qp_limits.py index adf3fbaf..5177baf5 100644 --- a/tests/test_qp_limits.py +++ b/tests/test_qp_limits.py @@ -26,6 +26,7 @@ from openarm_control import ArmSetup, pose_to_se3 from openarm_control.qp.arm_joint_limit import ( + SCALAR_JOINT_TYPES, ArmConfigurationLimit, ArmJointLimit, ) @@ -235,3 +236,21 @@ def test_target_does_not_change_geometric_ratio() -> None: np.testing.assert_allclose(shifted.G, initial.G, atol=1e-12) np.testing.assert_allclose(shifted.h, initial.h, atol=1e-12) + + +def test_scalar_joint_types_do_not_rely_on_mujoco_enum_equality() -> None: + setup = make_setup("right") + selected_qpos = { + int(index) for index in setup.joint_resolver.arm_qpos_indices("right") + } + selected_types = [ + setup.model.jnt_type[joint_id] + for joint_id in range(setup.model.njnt) + if int(setup.model.jnt_qposadr[joint_id]) in selected_qpos + ] + assert selected_types + + assert all(isinstance(joint_type, int) for joint_type in SCALAR_JOINT_TYPES) + for joint_type in selected_types: + assert int(joint_type) in SCALAR_JOINT_TYPES + assert int(mujoco.mjtJoint.mjJNT_FREE) not in SCALAR_JOINT_TYPES