# 双臂 MuJoCo 运动学遥操作实施计划 > **面向执行代理：** 必须逐任务执行本计划，并使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`。所有步骤使用复选框跟踪。 **目标：** 新增独立的 `xr_rm_mujoco` ROS2 包，用现有双臂 URDF 实时显示 Mock 或真机反馈，并保持现有 PICO、Placo QP、真机控制和安全行为不变。 **架构：** 左右 `single_arm_velocity_teleop` 节点继续负责所有控制逻辑，并用标准 `sensor_msgs/JointState` 发布当前适配器反馈和限速后的关节目标。单个 `dual_arm_simulator` 订阅左右反馈，按关节名写入 MuJoCo `qpos` 并调用 `mj_forward()`，只做运动学显示。现有 `arm_debug.launch.py` 用新增的 `use_mujoco` 参数选择是否启动显示进程。 **技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament_python、MuJoCo 3.10.0、Placo 0.9.4、NumPy、sensor_msgs、pytest、colcon。 --- ## 执行约束 - 所有构建、测试和启动命令均在 `/home/robot/WS_xr` 执行，并先运行： ```bash source /opt/ros/humble/setup.bash ``` - MuJoCo 和 Placo 测试使用 `/home/robot/miniconda3/envs/xr/bin/python`；不得用系统 pip、`pip --user` 或 `sudo pip` 改动现有环境。 - 自动化和启动验收只允许 `use_mock:=true`，不得连接真机、移动机械臂或操作夹爪。 - `move_to_initial_pose_on_connect` 保持 `false`，不得关闭现有安全限位、超时和停止 逻辑。 - 不修改左右节点名 `left_arm_teleop`、`right_arm_teleop`，不复制 URDF/mesh，不 生成持久化 MJCF，不实现动力学、碰撞或执行器。 - 每个任务只提交列出的文件，不提交无关工作树内容，不推送远程。 ## 文件结构 **新建：** - `xr_rm_mujoco/package.xml`：ROS2 包依赖。 - `xr_rm_mujoco/setup.py`：包安装和 `dual_arm_simulator` 入口。 - `xr_rm_mujoco/setup.cfg`：ament_python 脚本安装位置。 - `xr_rm_mujoco/resource/xr_rm_mujoco`：ament 索引标记。 - `xr_rm_mujoco/xr_rm_mujoco/__init__.py`：Python 包标记。 - `xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py`：MuJoCo 模型映射、ROS 订阅和 viewer。 - `xr_rm_mujoco/test/test_dual_arm_simulator.py`：模型加载、映射、校验与配置测试。 - `xr_rm_bringup/config/dual_arm_mujoco.yaml`：MuJoCo 专属刷新参数。 - `xr_rm_bringup/test/test_arm_debug_launch.py`：launch 参数和模式约束测试。 **修改：** - `xr_rm_teleop/package.xml`：增加 `sensor_msgs` 运行依赖。 - `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：只读暴露当前侧关节名称。 - `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：发布关节反馈/目标并调整 Mock Reset 后的重新锚定。 - `xr_rm_teleop/test/test_joint_control.py`：关节话题和 Reset 回归测试。 - `xr_rm_bringup/package.xml`：增加 `xr_rm_mujoco` 运行依赖。 - `xr_rm_bringup/launch/arm_debug.launch.py`：增加 `use_mujoco` 和仿真节点。 - `README.md`：记录目录、启动方式、频率、话题和 Reset 行为。 `xr_rm_bringup/CMakeLists.txt` 已安装整个 `config` 目录，无需修改。 ### 任务一：建立 MuJoCo 双臂运动学模型 **文件：** - 新建：`xr_rm_mujoco/package.xml` - 新建：`xr_rm_mujoco/setup.py` - 新建：`xr_rm_mujoco/setup.cfg` - 新建：`xr_rm_mujoco/resource/xr_rm_mujoco` - 新建：`xr_rm_mujoco/xr_rm_mujoco/__init__.py` - 新建：`xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py` - 新建：`xr_rm_mujoco/test/test_dual_arm_simulator.py` - [ ] **步骤 1：先写失败的 MuJoCo 模型测试** 创建 `xr_rm_mujoco/test/test_dual_arm_simulator.py`： ```python 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) ``` - [ ] **步骤 2：运行测试并确认因新包不存在而失败** 运行： ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash PYTHONPATH=src/xr_rm_mujoco \ /home/robot/miniconda3/envs/xr/bin/python -m pytest \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py -v ``` 预期：收集失败，提示 `ModuleNotFoundError: No module named 'xr_rm_mujoco'` 或缺少 `dual_arm_simulator`。 - [ ] **步骤 3：创建最小 ROS2 Python 包元数据** 创建空文件 `xr_rm_mujoco/resource/xr_rm_mujoco` 和 `xr_rm_mujoco/xr_rm_mujoco/__init__.py`。 创建 `xr_rm_mujoco/setup.cfg`： ```ini [develop] script_dir=$base/lib/xr_rm_mujoco [install] install_scripts=$base/lib/xr_rm_mujoco ``` 创建 `xr_rm_mujoco/setup.py`： ```python """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/package.xml`： ```xml 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 ``` 不要把 `mujoco` 加入 `install_requires`，避免 colcon 构建时通过 pip 改动已经固定的 Conda 环境；运行入口后续继续使用项目现有 `XR_PYTHON`。 - [ ] **步骤 4：实现最小的名称映射和运动学状态类** 创建 `xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py`： ```python """使用现有双 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] ] ``` - [ ] **步骤 5：运行 MuJoCo 模型测试并确认通过** 运行步骤 2 的同一命令。 预期：全部 PASS；本机输出会使用 MuJoCo 3.10.0，模型为 `nq=14`、`nv=14`。 - [ ] **步骤 6：提交新包基础** ```bash git add \ src/xr_rm_mujoco/package.xml \ src/xr_rm_mujoco/setup.py \ src/xr_rm_mujoco/setup.cfg \ src/xr_rm_mujoco/resource/xr_rm_mujoco \ src/xr_rm_mujoco/xr_rm_mujoco/__init__.py \ src/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py git commit -m "feat: 添加双臂 MuJoCo 运动学模型" ``` ### 任务二：发布左右关节反馈与限速目标 **文件：** - 修改：`xr_rm_teleop/package.xml` - 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py` - 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - 修改：`xr_rm_teleop/test/test_joint_control.py` - [ ] **步骤 1：先增加关节消息失败测试和测试发布器** 在 `xr_rm_teleop/test/test_joint_control.py` 导入： ```python from builtin_interfaces.msg import Time as TimeMsg ``` 给现有 `FakeTime` 增加： ```python def to_msg(self): return TimeMsg() ``` 在文件前部增加： ```python class FakePublisher: def __init__(self) -> None: self.messages = [] def publish(self, message) -> None: self.messages.append(message) def _joint_publishing_teleop() -> SingleArmVelocityTeleop: names = [f"omnipic_joint_{index}" for index in range(1, 8)] teleop = object.__new__(SingleArmVelocityTeleop) teleop._ik_solver = SimpleNamespace( joint_names=names, update_joint_state=lambda joints: np.eye(4), ) teleop._joint_state_pub = FakePublisher() teleop._joint_target_pub = FakePublisher() teleop._active = False teleop._last_valid_joint_target = None teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime()) return teleop def test_reset_joint_state_publishes_named_feedback() -> None: teleop = _joint_publishing_teleop() positions = [0.1 * index for index in range(7)] snapshot = JointStateSnapshot(positions, time.monotonic()) teleop._reset_joint_state(snapshot) message = teleop._joint_state_pub.messages[-1] assert message.name == teleop._ik_solver.joint_names assert message.position == pytest.approx(positions) def test_sync_joint_feedback_publishes_each_sample() -> None: teleop = _joint_publishing_teleop() positions = [0.2] * 7 teleop._sync_joint_feedback( JointStateSnapshot(positions, time.monotonic()) ) assert len(teleop._joint_state_pub.messages) == 1 assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions) ``` 给现有 `_timeout_teleop()` 补充一个 `FakePublisher`，防止后续成功发送目标时测试对象 缺少新属性： ```python teleop._joint_state_pub = FakePublisher() teleop._joint_target_pub = FakePublisher() teleop._ik_solver.joint_names = [ f"omnipic_joint_{index}" for index in range(1, 8) ] teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime()) ``` 增加限速后目标发布测试： ```python def test_send_joint_target_publishes_limited_command() -> None: sent = [] teleop = _joint_publishing_teleop() teleop._adapter = SimpleNamespace( send_joint_target=lambda joints, follow: sent.append((list(joints), follow)) ) teleop._follow = False teleop._latest_joint_positions = [0.0] * 7 teleop._last_joint_command_target = [0.0] * 7 teleop._last_joint_command_velocity = [0.0] * 7 teleop._joint_command_max_speed = 1.0 teleop._joint_command_max_acceleration = 100.0 teleop._dt = 0.1 assert teleop._send_joint_target([0.5] * 7) assert len(sent) == 1 assert sent[0][0] == pytest.approx([0.1] * 7) assert sent[0][1] is False message = teleop._joint_target_pub.messages[-1] assert message.name == teleop._ik_solver.joint_names assert message.position == pytest.approx(teleop._last_joint_command_target) ``` 现有以下两个测试也会经过新的反馈发布路径，分别补齐 `joint_names`、`_joint_state_pub` 和可生成 ROS 时间戳的 fake clock： ```python # test_startup_joint_query_initializes_qp_and_command_history teleop._ik_solver = SimpleNamespace( joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)], update_joint_state=lambda joints: pose, ) teleop._joint_state_pub = FakePublisher() teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime()) # test_first_feedback_initializes_last_valid_target_without_solving teleop._ik_solver.joint_names = [ f"omnipic_joint_{index}" for index in range(1, 8) ] teleop._joint_state_pub = FakePublisher() teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime()) ``` `test_feedback_fault_blocks_grip_until_release` 的测试对象会调用 `_control_tick()` 并 进入 `_sync_joint_feedback()`，同样增加： ```python teleop._ik_solver.joint_names = [ f"omnipic_joint_{index}" for index in range(1, 8) ] teleop._joint_state_pub = FakePublisher() ``` - [ ] **步骤 2：运行新增测试并确认因尚未发布消息而失败** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest \ src/xr_rm_teleop/test/test_joint_control.py::test_reset_joint_state_publishes_named_feedback \ src/xr_rm_teleop/test/test_joint_control.py::test_sync_joint_feedback_publishes_each_sample \ src/xr_rm_teleop/test/test_joint_control.py::test_send_joint_target_publishes_limited_command \ -v ``` 预期：FAIL，反馈或目标发布器的消息列表仍为空。 - [ ] **步骤 3：暴露求解器当前侧关节名称** 在 `PlacoIkSolver` 的 `base_configuration` 属性前增加： ```python @property def joint_names(self) -> list[str]: return list(self._joint_names) ``` 返回副本，调用者不能修改求解器内部关节顺序。 - [ ] **步骤 4：创建发布器和标准 JointState 消息** 在 `single_arm_velocity_teleop.py` 导入： ```python from sensor_msgs.msg import JointState ``` 在创建 `PlacoIkSolver` 后、连接适配器前创建两个发布器，使启动首帧可以立即发布： ```python debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}" self._joint_state_pub = self.create_publisher( JointState, f"{debug_ns}/joint_states", 10, ) self._joint_target_pub = self.create_publisher( JointState, f"{debug_ns}/joint_target", 10, ) ``` 保留后续五个现有调试发布器，并复用已经计算的 `debug_ns`，不要重复赋值。 在 `_reset_joint_state()` 前增加： ```python def _publish_joint_positions(self, publisher, positions: list[float]) -> None: message = JointState() message.header.stamp = self.get_clock().now().to_msg() message.name = self._ik_solver.joint_names message.position = [float(value) for value in positions] publisher.publish(message) ``` 在 `_reset_joint_state()` 设置完状态后、返回前增加： ```python self._publish_joint_positions(self._joint_state_pub, positions) ``` 在 `_sync_joint_feedback()` 设置完 `_last_current_pose` 后增加： ```python self._publish_joint_positions( self._joint_state_pub, list(snapshot.positions), ) ``` 在 `_send_joint_target()` 成功更新 `_last_joint_command_velocity` 后增加： ```python self._publish_joint_positions( self._joint_target_pub, limited_target, ) ``` 这样 `joint_target` 是经过关节速度/加速度限制后真正交给当前适配器的目标，而不是 限速前的 QP 原始结果。 - [ ] **步骤 5：声明 ROS 标准消息依赖** 在 `xr_rm_teleop/package.xml` 的 `rclpy` 依赖后增加： ```xml sensor_msgs ``` - [ ] **步骤 6：运行关节控制测试并确认通过** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest src/xr_rm_teleop/test/test_joint_control.py -v ``` 预期：全部 PASS，包括启动首帧、90 Hz 反馈同步路径和限速后目标发布。 - [ ] **步骤 7：提交关节状态接口** ```bash git add \ src/xr_rm_teleop/package.xml \ src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \ src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ src/xr_rm_teleop/test/test_joint_control.py git commit -m "feat: 发布双臂关节状态与目标" ``` ### 任务三：让 Mock Reset 后立即重新锚定 **文件：** - 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - 修改：`xr_rm_teleop/test/test_joint_control.py` - [ ] **步骤 1：写 Mock 与真机 Reset 差异的失败测试** 把现有 `_primary_button_teleop` 改为接收适配器模式： ```python def _primary_button_teleop(*, use_mock=False, move_error=None): # 保留现有函数体，并在 teleop 初始化处增加： teleop._use_mock = use_mock ``` 保留现有 `test_primary_button_rising_edge_moves_once_and_resyncs()` 对真机语义的 `assert teleop._grip_rearm_required`，并新增： ```python def test_mock_primary_reset_can_reanchor_without_grip_release() -> None: teleop, events, _, snapshot = _primary_button_teleop(use_mock=True) teleop._on_controller(SimpleNamespace(primary=False)) teleop._on_controller(SimpleNamespace(primary=True)) assert events == [ ("stop", True), "move", "read", ("sync", snapshot), ] assert not teleop._grip_rearm_required def test_failed_mock_primary_reset_still_requires_grip_release() -> None: failure = RuntimeError("mock reset failed") teleop, _, _, _ = _primary_button_teleop( use_mock=True, move_error=failure, ) teleop._on_controller(SimpleNamespace(primary=False)) teleop._on_controller(SimpleNamespace(primary=True)) assert teleop._grip_rearm_required ``` - [ ] **步骤 2：运行新增测试并确认自动重新锚定测试失败** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest \ src/xr_rm_teleop/test/test_joint_control.py::test_mock_primary_reset_can_reanchor_without_grip_release \ src/xr_rm_teleop/test/test_joint_control.py::test_failed_mock_primary_reset_still_requires_grip_release \ -v ``` 预期：第一项 FAIL，因为当前成功 Reset 后仍设置 `_grip_rearm_required=True`；失败 路径测试 PASS。 - [ ] **步骤 3：缓存 use_mock 并只在成功 Mock Reset 后解除重新使能要求** 在参数读取区域保存： ```python self._use_mock = self._bool_parameter("use_mock") ``` 把 `_make_adapter()` 中的判断改为： ```python if self._use_mock: return MockRealManAdapter(initial_joint_pose) ``` 在 `_handle_initial_pose_button()` 中保持 Reset 前先锁存： ```python self._grip_rearm_required = True ``` 并只在 `move_to_initial_pose()`、`read_joint_state()` 和 `_reset_joint_state()` 全部成功 后增加： ```python if self._use_mock: self._grip_rearm_required = False ``` 不要更改异常分支。`_safe_stop(reset_active=True)` 已清除旧手柄/机械臂基准；下一次 90 Hz 控制周期看到 Grip 仍按下时，会走现有 `_enter_active_control()`，以 Reset 后 状态自动建立新基准。真机仍保留 `_grip_rearm_required=True`。 - [ ] **步骤 4：运行完整关节与姿态控制测试** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest src/xr_rm_teleop/test/test_joint_control.py -v pytest src/xr_rm_teleop/test/test_orientation_control.py -v ``` 预期：全部 PASS；Mock 成功 Reset 可立即重锚，真机和失败路径仍需 Grip 松开。 - [ ] **步骤 5：提交 Reset 行为** ```bash git add \ src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ src/xr_rm_teleop/test/test_joint_control.py git commit -m "feat: 支持 MuJoCo 双臂即时复位" ``` ### 任务四：增加 ROS MuJoCo viewer 节点和独立配置 **文件：** - 修改：`xr_rm_mujoco/setup.py` - 修改：`xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py` - 修改：`xr_rm_mujoco/test/test_dual_arm_simulator.py` - 新建：`xr_rm_bringup/config/dual_arm_mujoco.yaml` - [ ] **步骤 1：写独立 MuJoCo 配置的失败测试** 在 `test_dual_arm_simulator.py` 增加： ```python MUJOCO_CONFIG_PATH = ( SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_mujoco.yaml" ) def test_mujoco_config_contains_only_render_parameters() -> None: with MUJOCO_CONFIG_PATH.open(encoding="utf-8") as stream: parameters = yaml.safe_load(stream)["dual_arm_simulator"]["ros__parameters"] assert parameters == {"render_rate_hz": 60.0} ``` - [ ] **步骤 2：运行配置测试并确认文件不存在** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash PYTHONPATH=src/xr_rm_mujoco \ /home/robot/miniconda3/envs/xr/bin/python -m pytest \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py::test_mujoco_config_contains_only_render_parameters \ -v ``` 预期：FAIL，提示 `dual_arm_mujoco.yaml` 不存在。 - [ ] **步骤 3：创建 MuJoCo 专属配置** 创建 `xr_rm_bringup/config/dual_arm_mujoco.yaml`： ```yaml # 双 RM75 MuJoCo 运动学显示参数。初始姿态和控制限制仍由 dual_arm_rm75.yaml 管理。 dual_arm_simulator: ros__parameters: render_rate_hz: 60.0 ``` - [ ] **步骤 4：在同一生产文件中增加最小 ROS wrapper** 在 `dual_arm_simulator.py` 增加导入： ```python from typing import Callable import mujoco.viewer import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState ``` 在 `DualArmKinematicModel` 后增加： ```python STATE_TOPICS = { "left": "/xr_rm/left_rm75/joint_states", "right": "/xr_rm/right_rm75/joint_states", } class DualArmSimulator(Node): """订阅左右关节反馈并刷新一个 MuJoCo 双臂 viewer。""" def __init__( self, viewer_factory: Callable = mujoco.viewer.launch_passive, ) -> None: super().__init__("dual_arm_simulator") self.declare_parameter("robot_urdf_path", "") self.declare_parameter("render_rate_hz", 60.0) render_rate_hz = float(self.get_parameter("render_rate_hz").value) if not math.isfinite(render_rate_hz) or render_rate_hz <= 0.0: raise ValueError("render_rate_hz must be finite and > 0") self._kinematics = DualArmKinematicModel( str(self.get_parameter("robot_urdf_path").value) ) self._viewer_factory = viewer_factory self._viewer = None self._subscriptions = [ self.create_subscription( JointState, topic, lambda message, selected_arm=arm: self._on_joint_state( selected_arm, message, ), 10, ) for arm, topic in STATE_TOPICS.items() ] self.create_timer(1.0 / render_rate_hz, self._render) self.get_logger().info( "MuJoCo 双臂节点已启动，等待左右关节状态，" f"render_rate_hz={render_rate_hz:.1f}" ) def _on_joint_state(self, arm: str, message: JointState) -> None: try: if self._viewer is None: self._kinematics.apply_arm_state( arm, list(message.name), list(message.position), ) else: with self._viewer.lock(): self._kinematics.apply_arm_state( arm, list(message.name), list(message.position), ) except (RuntimeError, ValueError) as exc: self.get_logger().warn( f"拒绝 {arm} 关节状态：{exc}", throttle_duration_sec=1.0, ) def _render(self) -> None: for topic in STATE_TOPICS.values(): publisher_count = self.count_publishers(topic) if publisher_count > 1: self.get_logger().warn( f"关节状态话题存在多个发布者：{topic}, count={publisher_count}", throttle_duration_sec=5.0, ) if not self._kinematics.ready: return if self._viewer is None: self._viewer = self._viewer_factory( self._kinematics.model, self._kinematics.data, ) if not self._viewer.is_running(): self.get_logger().info("MuJoCo viewer 已关闭。") rclpy.shutdown() return self._viewer.sync() def close_viewer(self) -> None: if self._viewer is not None: self._viewer.close() self._viewer = None def main(args=None) -> None: rclpy.init(args=args) node = None try: node = DualArmSimulator() rclpy.spin(node) finally: if node is not None: node.close_viewer() node.destroy_node() if rclpy.ok(): rclpy.shutdown() if __name__ == "__main__": main() ``` 保留单线程 `rclpy.spin()`；ROS 回调和 render timer 不会并行修改 qpos，viewer 已启动 后通过官方 `viewer.lock()` 保护写入。 - [ ] **步骤 5：安装 ROS 可执行入口** 在 `xr_rm_mujoco/setup.py` 的 `tests_require` 后增加： ```python entry_points={ "console_scripts": [ "dual_arm_simulator = xr_rm_mujoco.dual_arm_simulator:main", ], }, ``` - [ ] **步骤 6：运行 MuJoCo 测试并确认通过** 运行： ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash PYTHONPATH=src/xr_rm_mujoco \ /home/robot/miniconda3/envs/xr/bin/python -m pytest \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py -v ``` 预期：全部 PASS；测试只加载模型，不打开 viewer。 - [ ] **步骤 7：提交 ROS viewer 与配置** ```bash git add \ src/xr_rm_mujoco/setup.py \ src/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py \ src/xr_rm_bringup/config/dual_arm_mujoco.yaml git commit -m "feat: 添加双臂 MuJoCo 显示节点" ``` ### 任务五：接入统一 launch 并保持默认行为 **文件：** - 新建：`xr_rm_bringup/test/test_arm_debug_launch.py` - 修改：`xr_rm_bringup/launch/arm_debug.launch.py` - 修改：`xr_rm_bringup/package.xml` - [ ] **步骤 1：写 launch 参数和范围约束的失败测试** 创建 `xr_rm_bringup/test/test_arm_debug_launch.py`： ```python import importlib.util from pathlib import Path import pytest from launch import LaunchContext from launch.actions import DeclareLaunchArgument from launch.utilities import perform_substitutions MODULE_PATH = Path(__file__).parents[1] / "launch" / "arm_debug.launch.py" SPEC = importlib.util.spec_from_file_location("arm_debug_launch", MODULE_PATH) arm_debug_launch = importlib.util.module_from_spec(SPEC) assert SPEC.loader is not None SPEC.loader.exec_module(arm_debug_launch) def test_launch_declares_mujoco_disabled_by_default() -> None: description = arm_debug_launch.generate_launch_description() arguments = { entity.name: entity for entity in description.entities if isinstance(entity, DeclareLaunchArgument) } assert "use_mujoco" in arguments assert perform_substitutions( LaunchContext(), arguments["use_mujoco"].default_value, ) == "false" def test_mujoco_mode_requires_both_arms() -> None: arm_debug_launch._validate_mujoco_mode("both", True) arm_debug_launch._validate_mujoco_mode("left", False) with pytest.raises(ValueError, match="arm:=both"): arm_debug_launch._validate_mujoco_mode("left", True) ``` - [ ] **步骤 2：运行 launch 测试并确认缺少参数和校验函数** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest src/xr_rm_bringup/test/test_arm_debug_launch.py -v ``` 预期：FAIL，提示缺少 `use_mujoco` 或 `_validate_mujoco_mode`。 - [ ] **步骤 3：增加 MuJoCo 节点构造与模式校验** 在 `arm_debug.launch.py` 的 `_udp_receiver_node()` 后增加： ```python def _mujoco_node() -> Node: """启动只读双臂 MuJoCo 运动学显示节点。""" return Node( package="xr_rm_mujoco", executable="dual_arm_simulator", name="dual_arm_simulator", output="screen", prefix=[XR_PYTHON], parameters=[ _config_file("dual_arm_mujoco.yaml"), {"robot_urdf_path": _dual_rm75_urdf()}, ], ) def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None: if use_mujoco and arm != "both": raise ValueError("use_mujoco:=true requires arm:=both") ``` 在 `_launch_setup()` 读取： ```python use_mujoco = _as_bool( LaunchConfiguration("use_mujoco").perform(context) ) ``` 在已有 `arm` 校验后调用： ```python _validate_mujoco_mode(arm, use_mujoco) ``` 在左右遥操作节点加入完成后追加： ```python if use_mujoco: nodes.append(_mujoco_node()) ``` 在 `generate_launch_description()` 中 `use_mock` 后增加： ```python # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。 DeclareLaunchArgument("use_mujoco", default_value="false"), ``` - [ ] **步骤 4：声明 bringup 对新包的运行依赖** 在 `xr_rm_bringup/package.xml` 的 `xr_rm_input` 后增加： ```xml xr_rm_mujoco ``` - [ ] **步骤 5：运行 launch 测试并确认通过** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash pytest src/xr_rm_bringup/test/test_arm_debug_launch.py -v ``` 预期：全部 PASS；`use_mujoco` 默认关闭，只有 `arm:=both` 可以启用。 - [ ] **步骤 6：提交 launch 集成** ```bash git add \ src/xr_rm_bringup/package.xml \ src/xr_rm_bringup/launch/arm_debug.launch.py \ src/xr_rm_bringup/test/test_arm_debug_launch.py git commit -m "feat: 接入双臂 MuJoCo 启动模式" ``` ### 任务六：更新文档并完成全量 Mock 验收 **文件：** - 修改：`README.md` - [ ] **步骤 1：更新 README 的范围、结构与运行说明** 在 README 中做以下明确修改： 1. 在“已完成”加入 MuJoCo 运动学显示； 2. 在结构树加入 `xr_rm_mujoco` 和 `dual_arm_mujoco.yaml`； 3. 在 launch 参数加入 `use_mujoco`； 4. 增加下面两条命令，并明确第二条会连接真机： ```bash # 无真机：Mock 状态驱动 MuJoCo ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=both use_mock:=true use_mujoco:=true # 真机：实际反馈同步到 MuJoCo ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=both use_mock:=false use_mujoco:=true ``` 5. 记录关节反馈话题、目标话题、60 Hz viewer、90 Hz MuJoCo 状态输入和真机原始 200 Hz 反馈； 6. 记录左 X/右 A Reset：Mock 立即复位且 Grip 保持时自动重锚，真机仍需松开 Grip； 7. 明确 `move_to_initial_pose_on_connect` 继续保持 `false`。 - [ ] **步骤 2：运行局部测试** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash PYTHONPATH=src/xr_rm_mujoco \ /home/robot/miniconda3/envs/xr/bin/python -m pytest \ src/xr_rm_mujoco/test/test_dual_arm_simulator.py -v pytest src/xr_rm_teleop/test/test_joint_control.py -v pytest src/xr_rm_teleop/test/test_orientation_control.py -v pytest src/xr_rm_bringup/test/test_arm_debug_launch.py -v ``` 预期：全部 PASS，不允许把跳过或收集失败报告为通过。 - [ ] **步骤 3：构建整个工作空间并检查安装资源** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash colcon build --symlink-install source install/setup.bash ros2 pkg executables xr_rm_mujoco test -f install/xr_rm_bringup/share/xr_rm_bringup/config/dual_arm_mujoco.yaml ``` 预期：构建成功；`ros2 pkg executables` 显示 `xr_rm_mujoco dual_arm_simulator`；配置文件检查返回 0。 - [ ] **步骤 4：验证原有 Mock 默认路径不启动 MuJoCo** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash source install/setup.bash timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=both use_mock:=true use_mujoco:=false ``` 预期：左右 `left_arm_teleop`、`right_arm_teleop` 和 UDP receiver 正常启动，没有 `dual_arm_simulator`；`timeout` 返回 124 属于预期。 - [ ] **步骤 5：在有图形桌面的终端验证 MuJoCo Mock 链路** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash source install/setup.bash ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=both use_mock:=true use_mujoco:=true ``` 另开终端启动 PICO bridge 或现有 sample sender。确认： - viewer 收齐左右首帧后以 `dual_arm_rm75.yaml` 初始姿态打开； - 左右 Grip 分别驱动对应机械臂，不串臂； - 左 X、右 A 分别立即 Reset，Grip 保持时下一周期重新锚定并可继续运动； - `ros2 topic hz /xr_rm/left_rm75/joint_states` 和右侧话题接近 `90 Hz`； - 关闭 viewer 只结束 MuJoCo 节点，不改变遥操作节点。 不得为完成本步骤使用 `use_mock:=false`。如果当前会话没有图形桌面，记录 “未执行 viewer 人工检查：无 DISPLAY”，但模型测试、构建和非 viewer Mock 启动仍 必须完成。 - [ ] **步骤 6：检查安全配置未被改变** ```bash cd /home/robot/WS_xr rg -n \ "configure_safety_limits: true|move_to_initial_pose_on_connect: false|control_rate_hz: 90.0" \ src/xr_rm_bringup/config/dual_arm_rm75.yaml \ src/xr_rm_bringup/config/left_arm_rm75.yaml \ src/xr_rm_bringup/config/right_arm_rm75.yaml ``` 预期：三份配置仍保留安全限位、禁用连接即移动，并保持 90 Hz 控制频率；本任务只 新增 `dual_arm_mujoco.yaml`，不改这些值。 - [ ] **步骤 7：提交 README** ```bash git add src/README.md git commit -m "docs: 补充双臂 MuJoCo 使用说明" ``` ## 完成标准 - `Dual_arm.urdf` 是 MuJoCo 和 Placo 唯一双臂模型源； - `use_mujoco` 默认关闭，现有 mock/真机命令行为不变； - Mock 和真机模式的 MuJoCo 输入均为当前适配器反馈，名义发布频率 90 Hz； - 真机原始反馈保持 5 ms 周期，MuJoCo viewer 为 60 Hz； - Mock A/X Reset 立即回 YAML 初始姿态并可在 Grip 保持时重新锚定； - 真机 Reset、安全限位、超时、停止和单 RealMan 连接行为不变； - 所有指定测试、colcon 构建和安装资源检查通过； - 未执行任何真机运动、夹爪操作、远程提交或推送。