Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 9 additions & 8 deletions src/openarm_control/qp/arm_joint_limit.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand All @@ -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)
Expand Down Expand Up @@ -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:
Expand Down
19 changes: 19 additions & 0 deletions tests/test_qp_limits.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
)
Expand Down Expand Up @@ -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