acRealman_xr/docs/superpowers/plans/2026-08-04-dual-arm-mujoco-teleoperation.md

# 双臂 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
<?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>
```

不要把 `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
  <exec_depend>sensor_msgs</exec_depend>
```

- [ ] **步骤 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
  <exec_depend>xr_rm_mujoco</exec_depend>
```

- [ ] **步骤 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 构建和安装资源检查通过；
- 未执行任何真机运动、夹爪操作、远程提交或推送。