feat: 添加双臂 MuJoCo 运动学模型

xr_rm_mujoco/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>xr_rm_mujoco</name>
+  <version>0.1.0</version>
+  <description>MuJoCo kinematic visualization for the dual RM75 platform.</description>
+  <maintainer email="user@example.com">Yikai Fu</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>xr_rm_teleop</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>python3-yaml</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

xr_rm_mujoco/resource/xr_rm_mujoco

@@ -0,0 +1 @@
+

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

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"],
+)

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)

xr_rm_mujoco/xr_rm_mujoco/__init__.py

@@ -0,0 +1 @@
+"""双 RM75 MuJoCo 运动学显示包。"""

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]
+        ]