diff --git a/docs/superpowers/plans/2026-08-04-dual-arm-mujoco-teleoperation.md b/docs/superpowers/plans/2026-08-04-dual-arm-mujoco-teleoperation.md new file mode 100644 index 0000000..65bff0b --- /dev/null +++ b/docs/superpowers/plans/2026-08-04-dual-arm-mujoco-teleoperation.md @@ -0,0 +1,1231 @@ +# 双臂 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 构建和安装资源检查通过； +- 未执行任何真机运动、夹爪操作、远程提交或推送。 diff --git a/docs/superpowers/specs/2026-08-04-dual-arm-mujoco-teleoperation-design.md b/docs/superpowers/specs/2026-08-04-dual-arm-mujoco-teleoperation-design.md new file mode 100644 index 0000000..1b9ac3c --- /dev/null +++ b/docs/superpowers/specs/2026-08-04-dual-arm-mujoco-teleoperation-design.md @@ -0,0 +1,249 @@ +# 双臂 MuJoCo 运动学遥操作设计 + +## 背景与目标 + +当前项目已经通过 PICO/XR 手柄、两个独立的单臂遥操作节点和 Placo QP 完成双 +RM75 遥操作。左右节点共同加载 +`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`，但现有 `use_mock:=true` 只在内存中 +保存关节状态，没有可视化模型。 + +本次变更增加一个独立的 MuJoCo 运动学仿真包，使双臂在不连接真机时可以由 PICO +遥操作并可视化，也允许连接真机时把实际关节反馈同步显示在 MuJoCo 中。仿真用于更 +方便地观察和改进现有 QP 算法，不替代现有控制与安全链路。 + +首版目标： + +- 直接加载现有双臂 URDF，保持它是唯一模型源； +- 复用现有 PICO 输入、目标生成、工作空间限制和 Placo QP； +- 使用一个 MuJoCo 进程显示完整 14 关节双臂模型； +- 无真机时显示 Mock 关节状态，连接真机时显示实际关节反馈； +- 支持 Mock 模式下用左手 X、右手 A 立即 Reset 对应机械臂； +- 保持当前 mock、真机和夹爪功能的默认行为不变。 + +首版不实现 MuJoCo 动力学、执行器、接触、碰撞约束、双臂协同 QP、轨迹记录或 +MuJoCo 对真机的任何控制。 + +## 目录与包边界 + +新增独立的 `ament_python` 包 `xr_rm_mujoco`，运行配置仍统一由 +`xr_rm_bringup` 管理： + +```text +src/ +├── xr_rm_mujoco/ +│ ├── package.xml +│ ├── setup.py +│ ├── setup.cfg +│ ├── resource/ +│ │ └── xr_rm_mujoco +│ ├── xr_rm_mujoco/ +│ │ ├── __init__.py +│ │ └── dual_arm_simulator.py +│ └── test/ +│ └── test_dual_arm_simulator.py +├── xr_rm_bringup/ +│ ├── config/ +│ │ ├── dual_arm_rm75.yaml +│ │ └── dual_arm_mujoco.yaml +│ └── launch/ +│ └── arm_debug.launch.py +└── xr_rm_teleop/ + ├── models/ + │ └── dual_rm75/ + │ └── Dual_arm.urdf + └── xr_rm_teleop/ + └── single_arm_velocity_teleop.py +``` + +各部分职责： + +- `xr_rm_mujoco` 只加载模型、接收关节状态、更新 MuJoCo `qpos` 和刷新画面； +- `xr_rm_teleop` 继续负责 PICO 映射、目标滤波、安全限幅、QP 和适配器选择，只 + 增加关节目标及当前关节状态发布； +- `xr_rm_bringup` 保持唯一遥操作 launch 入口，并保存 MuJoCo 运行参数； +- `Dual_arm.urdf` 和现有 meshes 保持原位置，不复制或生成持久化 MJCF； +- 不拆出新的 description 包，不增加第二套遥操作实现。 + +## 模型与 MuJoCo 更新方式 + +`dual_arm_simulator` 从安装空间解析 +`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`，MuJoCo 直接加载该文件及其相对路径 +网格。节点按 URDF 关节名称查找 MuJoCo qpos 地址，不写死 14 个数组下标。 + +左右首帧合法关节状态到达后，节点把状态写入相应 `qpos`，调用 `mj_forward()` +更新运动学，再由被动 viewer 显示。首版不调用 `mj_step()` 推进动力学，MuJoCo +不会生成控制量或新的关节运动。 + +画面按 `60 Hz` 刷新。`xr_rm_bringup/config/dual_arm_mujoco.yaml` 首版只包含： + +```yaml +dual_arm_simulator: + ros__parameters: + render_rate_hz: 60.0 +``` + +初始关节角不在该文件中重复配置。 + +## ROS 话题与状态来源 + +左右遥操作节点使用标准 `sensor_msgs/msg/JointState` 发布： + +| 话题 | 内容 | +|---|---| +| `/xr_rm/left_rm75/joint_states` | 左臂当前适配器反馈 | +| `/xr_rm/right_rm75/joint_states` | 右臂当前适配器反馈 | +| `/xr_rm/left_rm75/joint_target` | 左臂经关节限速后实际下发的目标 | +| `/xr_rm/right_rm75/joint_target` | 右臂经关节限速后实际下发的目标 | + +MuJoCo 只订阅两个 `joint_states` 话题。`joint_target` 用于后续记录和比较，不驱动 +MuJoCo。消息必须携带对应侧完整的 7 个关节名称和位置，MuJoCo 按名称映射，不能 +依赖消息数组顺序。 + +状态来源由现有 `use_mock` 唯一决定： + +```text +use_mock:=true +PICO → Placo QP → MockRealManAdapter → joint_states → MuJoCo + +use_mock:=false +PICO → Placo QP → RealManAdapter → 真机 + 真机实时反馈 → joint_states → MuJoCo +``` + +每个遥操作节点只创建一种适配器。真机连接或反馈失败时不得创建、切换或回退到 +Mock。MuJoCo 不需要独立的状态来源参数；同一状态话题发现多个发布者时输出明确 +报警，防止同时运行两套 launch 造成状态混合。 + +## 更新频率 + +两侧 `dual_arm_rm75.yaml` 的 `control_rate_hz` 均为 `90.0`： + +- Mock 模式：Mock 状态在遥操作节点的 `90 Hz` 控制周期中读取并发布，MuJoCo + 名义关节接收频率为 `90 Hz`； +- 真机模式：RealMan 的 `realtime_push_cycle_ms: 5` 使适配器原始反馈名义频率为 + `200 Hz`，遥操作节点在 `90 Hz` 控制周期取最新快照并发布，因此 MuJoCo 名义 + 关节接收频率仍为 `90 Hz`； +- 画面独立按 `render_rate_hz: 60.0` 刷新，每帧显示当时最新的 14 关节状态。 + +以上是名义频率，实际频率会受系统调度影响，运行时使用 `ros2 topic hz` 检查。 + +## 初始姿态与 A/X Reset + +`dual_arm_rm75.yaml` 继续作为双臂初始姿态和控制限制的唯一配置源。Mock 适配器 +创建时已经读取对应节点的 `initial_joint_pose`，将角度转换成弧度并作为初始关节 +状态。左右遥操作节点初始化完成后立即各发布一帧状态，因此无真机 MuJoCo 的默认 +姿态就是 YAML 中的左右初始姿态。 + +现有 `XrController.primary` 和按键上升沿逻辑继续复用： + +```text +左手 X → 左臂立即 Reset 到左臂 initial_joint_pose +右手 A → 右臂立即 Reset 到右臂 initial_joint_pose +同时按 X、A → 双臂分别立即 Reset +``` + +Mock Reset 不生成平滑轨迹，而是立即更新对应 7 个关节并发布新状态。Reset 前先 +退出旧的相对位姿控制；如果 Grip 仍保持按下，下一控制周期使用“当前手柄姿态 + +Reset 后机械臂姿态”自动建立新基准，随后可以继续遥操作，不要求先松开 Grip， +也不能沿用 Reset 前的相对位姿基准。 + +真机的 A/X 回位行为保持现状：调用 RealMan 初始位姿运动，完成后重新同步反馈， +并要求先松开 Grip 才能重新使能。该差异只由 `use_mock` 决定。 + +三份 RM75 配置中的 `move_to_initial_pose_on_connect` 默认继续保持 `false`。MuJoCo +初始显示和按键 Reset 都不依赖该开关，连接真机时不得默认自动移动双臂。 + +## 控制限制与安全隔离 + +Mock + MuJoCo 继续执行 `dual_arm_rm75.yaml` 中现有的软件控制约束： + +- `workspace_min`、`workspace_max`、`cyl_radius_limit` 和低位圆柱限制； +- `max_linear_speed` 和 `max_orientation_speed`； +- `joint_max_speed` 和 `joint_max_acc`； +- Placo 的关节位置、速度和求解收敛检查； +- Grip 运动门控、XR/反馈超时、QP 失败保持和安全停止。 + +MuJoCo 直接显示已经受限的离散关节状态，本身不额外模拟连续动力学。 +`max_line_speed`、`max_angular_speed`、`max_line_acc`、`max_angular_acc` 以及 +`configure_safety_limits` 是 RealMan 控制器配置，只在真机适配器中调用；这不影响 +上述对 Mock 同样生效的软件限位。 + +MuJoCo 节点只订阅状态，不发布机器人控制指令，不导入 RealMan SDK，也不创建新的 +RealMan 连接。MuJoCo 启动失败、运行异常或窗口关闭不得改变真机命令、安全停止或 +夹爪行为。 + +## 启动设计 + +继续使用唯一入口 `xr_rm_bringup/launch/arm_debug.launch.py`，增加默认关闭的 +`use_mujoco` 参数： + +| `use_mock` | `use_mujoco` | 行为 | +|---|---|---| +| `true` | `false` | 现有内存 Mock，无 MuJoCo | +| `true` | `true` | Mock + MuJoCo 双臂显示 | +| `false` | `false` | 现有双臂真机遥操作 | +| `false` | `true` | 双臂真机遥操作 + 实际反馈同步显示 | + +`use_mujoco` 不参与适配器选择。首版只接受 +`arm:=both use_mujoco:=true`，避免单臂启动时另一侧状态和初始姿态不明确。 + +无真机使用方式： + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py \ + arm:=both use_mock:=true use_mujoco:=true +``` + +真机同步显示方式： + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py \ + arm:=both use_mock:=false use_mujoco:=true +``` + +第二条命令会连接并控制真机，只能在完成现有真机安全检查后使用。所有自动化和首次 +集成验收只运行 `use_mock:=true`。 + +MuJoCo 进程使用项目现有的 XR Conda Python，因为本机 MuJoCo 与 Placo 均安装在 +该环境中。未启用 `use_mujoco` 时不启动或导入 MuJoCo，新包不能让现有 mock 模式 +强制依赖厂商 SDK。 + +## 校验与异常处理 + +- URDF、网格或 MuJoCo 加载失败：MuJoCo 节点明确报错并退出，现有遥操节点不改变； +- 收到关节缺失、重复、数量错误或包含 NaN/Inf 的消息：拒绝整帧并保持上一姿态； +- 尚未收齐左右首帧状态：等待并报告缺失侧，不把零位姿冒充有效初始姿态； +- 任一侧状态暂时中断：保持该侧最后有效姿态，不生成运动、不切换来源； +- 同一状态话题存在多个发布者：输出明确报警； +- viewer 关闭：只结束 MuJoCo 显示，不触发或改变机器人运动。 + +## 测试与验收 + +使用现有 pytest、ROS2 Humble 和 colcon，不增加测试框架，不连接真机。 + +最小自动化覆盖： + +- MuJoCo 可以直接加载现有双臂 URDF； +- 14 个活动关节名称与左右 qpos 映射正确，消息顺序变化不会串臂； +- YAML 初始角度经 Mock 转换后能正确写入 MuJoCo； +- 非法关节消息不会部分污染当前状态； +- Mock A/X Reset 后回到对应 YAML 姿态； +- Reset 时 Grip 保持按下能够重新锚定并继续控制； +- 真机路径仍保留 Grip 松开后重新使能要求； +- `use_mujoco` 默认关闭，现有三种 mock/真机启动行为不变。 + +在工作空间根目录执行： + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +/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 +colcon build --symlink-install +``` + +构建后只用 Mock 启动并通过 PICO 或 sample UDP 检查：左右模型初始姿态、独立运动、 +A/X Reset、Reset 后继续遥操作、话题频率和关闭 viewer 后遥操作节点状态。不得在 +自动化验收中使用 `use_mock:=false`。