diff --git a/xr_rm_mujoco/package.xml b/xr_rm_mujoco/package.xml new file mode 100644 index 0000000..427e4d0 --- /dev/null +++ b/xr_rm_mujoco/package.xml @@ -0,0 +1,24 @@ + + + + xr_rm_mujoco + 0.1.0 + MuJoCo kinematic visualization for the dual RM75 platform. + Yikai Fu + Apache-2.0 + + ament_python + + rclpy + sensor_msgs + xr_rm_teleop + + ament_lint_auto + ament_lint_common + python3-pytest + python3-yaml + + + ament_python + + diff --git a/xr_rm_mujoco/resource/xr_rm_mujoco b/xr_rm_mujoco/resource/xr_rm_mujoco new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/xr_rm_mujoco/resource/xr_rm_mujoco @@ -0,0 +1 @@ + diff --git a/xr_rm_mujoco/setup.cfg b/xr_rm_mujoco/setup.cfg new file mode 100644 index 0000000..83772c8 --- /dev/null +++ b/xr_rm_mujoco/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/xr_rm_mujoco +[install] +install_scripts=$base/lib/xr_rm_mujoco diff --git a/xr_rm_mujoco/setup.py b/xr_rm_mujoco/setup.py new file mode 100644 index 0000000..ecfb22a --- /dev/null +++ b/xr_rm_mujoco/setup.py @@ -0,0 +1,23 @@ +"""MuJoCo 双 RM75 运动学显示包安装配置。""" + +from setuptools import setup + + +package_name = "xr_rm_mujoco" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Yikai Fu", + maintainer_email="user@example.com", + description="MuJoCo kinematic visualization for the dual RM75 platform.", + license="Apache-2.0", + tests_require=["pytest"], +) diff --git a/xr_rm_mujoco/test/test_dual_arm_simulator.py b/xr_rm_mujoco/test/test_dual_arm_simulator.py new file mode 100644 index 0000000..10799a8 --- /dev/null +++ b/xr_rm_mujoco/test/test_dual_arm_simulator.py @@ -0,0 +1,92 @@ +import math +from pathlib import Path + +import pytest +import yaml + +from xr_rm_mujoco.dual_arm_simulator import ( + ARM_JOINT_NAMES, + DualArmKinematicModel, +) + + +SRC_DIR = Path(__file__).resolve().parents[2] +URDF_PATH = ( + SRC_DIR / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf" +) +DUAL_CONFIG_PATH = SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml" + + +def test_dual_urdf_loads_with_expected_joint_mapping() -> None: + simulation = DualArmKinematicModel(str(URDF_PATH)) + + assert simulation.model.nq == 14 + assert simulation.model.nv == 14 + assert ARM_JOINT_NAMES == { + "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)), + "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)), + } + assert not simulation.ready + + +def test_joint_messages_are_mapped_by_name_not_array_order() -> None: + simulation = DualArmKinematicModel(str(URDF_PATH)) + names = list(reversed(ARM_JOINT_NAMES["left"])) + values = [float(index) / 10.0 for index in range(7)] + + simulation.apply_arm_state("left", names, values) + + by_name = dict(zip(names, values)) + assert simulation.joint_positions("left") == pytest.approx( + [by_name[name] for name in ARM_JOINT_NAMES["left"]] + ) + assert not simulation.ready + + +def test_yaml_initial_poses_populate_both_arms() -> None: + simulation = DualArmKinematicModel(str(URDF_PATH)) + with DUAL_CONFIG_PATH.open(encoding="utf-8") as stream: + config = yaml.safe_load(stream) + + for arm, node_name in ( + ("left", "left_arm_teleop"), + ("right", "right_arm_teleop"), + ): + degrees = config[node_name]["ros__parameters"]["initial_joint_pose"] + radians = [math.radians(value) for value in degrees] + simulation.apply_arm_state(arm, ARM_JOINT_NAMES[arm], radians) + assert simulation.joint_positions(arm) == pytest.approx(radians) + + assert simulation.ready + + +@pytest.mark.parametrize( + ("names", "positions", "match"), + [ + (list(ARM_JOINT_NAMES["left"][:-1]), [0.0] * 6, "expected"), + (list(ARM_JOINT_NAMES["left"]), [0.0] * 6, "same length"), + ( + [ARM_JOINT_NAMES["left"][0]] * 7, + [0.0] * 7, + "unique", + ), + ( + list(ARM_JOINT_NAMES["left"]), + [0.0] * 6 + [math.nan], + "finite", + ), + ], +) +def test_invalid_joint_state_is_rejected_without_partial_update( + names: list[str], + positions: list[float], + match: str, +) -> None: + simulation = DualArmKinematicModel(str(URDF_PATH)) + valid = [0.1] * 7 + simulation.apply_arm_state("left", ARM_JOINT_NAMES["left"], valid) + + with pytest.raises(ValueError, match=match): + simulation.apply_arm_state("left", names, positions) + + assert simulation.joint_positions("left") == pytest.approx(valid) diff --git a/xr_rm_mujoco/xr_rm_mujoco/__init__.py b/xr_rm_mujoco/xr_rm_mujoco/__init__.py new file mode 100644 index 0000000..89289d0 --- /dev/null +++ b/xr_rm_mujoco/xr_rm_mujoco/__init__.py @@ -0,0 +1 @@ +"""双 RM75 MuJoCo 运动学显示包。""" diff --git a/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py b/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py new file mode 100644 index 0000000..57a78ff --- /dev/null +++ b/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py @@ -0,0 +1,86 @@ +"""使用现有双 RM75 URDF 的 MuJoCo 运动学状态映射。""" + +from __future__ import annotations + +import math +from pathlib import Path + +import mujoco + + +ARM_JOINT_NAMES = { + "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)), + "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)), +} + + +class DualArmKinematicModel: + """加载双臂 URDF，并按关节名更新 MuJoCo qpos。""" + + def __init__(self, urdf_path: str) -> None: + path = Path(urdf_path).expanduser().resolve() + if not path.is_file(): + raise FileNotFoundError(f"dual RM75 URDF not found: {path}") + + self.model = mujoco.MjModel.from_xml_path(str(path)) + self.data = mujoco.MjData(self.model) + self._qpos_addresses: dict[str, dict[str, int]] = {} + self._received_arms: set[str] = set() + + for arm, names in ARM_JOINT_NAMES.items(): + addresses = {} + for name in names: + joint_id = mujoco.mj_name2id( + self.model, + mujoco.mjtObj.mjOBJ_JOINT, + name, + ) + if joint_id < 0: + raise RuntimeError(f"MuJoCo joint not found: {name}") + if self.model.jnt_type[joint_id] != mujoco.mjtJoint.mjJNT_HINGE: + raise RuntimeError(f"MuJoCo joint must be hinge: {name}") + addresses[name] = int(self.model.jnt_qposadr[joint_id]) + self._qpos_addresses[arm] = addresses + + @property + def ready(self) -> bool: + return self._received_arms == set(ARM_JOINT_NAMES) + + def apply_arm_state( + self, + arm: str, + names: list[str] | tuple[str, ...], + positions: list[float] | tuple[float, ...], + ) -> None: + if arm not in ARM_JOINT_NAMES: + raise ValueError("arm must be left or right") + if len(names) != len(positions): + raise ValueError("joint names and positions must have the same length") + if len(set(names)) != len(names): + raise ValueError("joint names must be unique") + + expected = set(ARM_JOINT_NAMES[arm]) + if set(names) != expected: + raise ValueError(f"joint names must match expected {arm} joints") + + values = [float(value) for value in positions] + if not all(math.isfinite(value) for value in values): + raise ValueError("joint positions must be finite") + + by_name = dict(zip(names, values)) + updates = [ + (self._qpos_addresses[arm][name], by_name[name]) + for name in ARM_JOINT_NAMES[arm] + ] + for address, value in updates: + self.data.qpos[address] = value + mujoco.mj_forward(self.model, self.data) + self._received_arms.add(arm) + + def joint_positions(self, arm: str) -> list[float]: + if arm not in ARM_JOINT_NAMES: + raise ValueError("arm must be left or right") + return [ + float(self.data.qpos[self._qpos_addresses[arm][name]]) + for name in ARM_JOINT_NAMES[arm] + ]