YikaiFu-cart/acRealman_xr

Fork 1

Files

T

YikaiFu-cart 1df09fef63 docs: 添加双臂 MuJoCo 设计与计划

2026-08-04 11:42:07 +08:00

38 KiB

Raw Blame History

双臂 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 执行，并先运行：
```
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：

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：运行测试并确认因新包不存在而失败

运行：

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：

[develop]
script_dir=$base/lib/xr_rm_mujoco
[install]
install_scripts=$base/lib/xr_rm_mujoco

创建 xr_rm_mujoco/setup.py：

"""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 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：

"""使用现有双 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：提交新包基础

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 导入：

from builtin_interfaces.msg import Time as TimeMsg

给现有 FakeTime 增加：

    def to_msg(self):
        return TimeMsg()

在文件前部增加：

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，防止后续成功发送目标时测试对象缺少新属性：

    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())

增加限速后目标发布测试：

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：

# 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()，同样增加：

teleop._ik_solver.joint_names = [
    f"omnipic_joint_{index}" for index in range(1, 8)
]
teleop._joint_state_pub = FakePublisher()

步骤 2：运行新增测试并确认因尚未发布消息而失败

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 属性前增加：

    @property
    def joint_names(self) -> list[str]:
        return list(self._joint_names)

返回副本，调用者不能修改求解器内部关节顺序。

步骤 4：创建发布器和标准 JointState 消息

在 single_arm_velocity_teleop.py 导入：

from sensor_msgs.msg import JointState

在创建 PlacoIkSolver 后、连接适配器前创建两个发布器，使启动首帧可以立即发布：

        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() 前增加：

    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() 设置完状态后、返回前增加：

        self._publish_joint_positions(self._joint_state_pub, positions)

在 _sync_joint_feedback() 设置完 _last_current_pose 后增加：

        self._publish_joint_positions(
            self._joint_state_pub,
            list(snapshot.positions),
        )

在 _send_joint_target() 成功更新 _last_joint_command_velocity 后增加：

        self._publish_joint_positions(
            self._joint_target_pub,
            limited_target,
        )

这样 joint_target 是经过关节速度/加速度限制后真正交给当前适配器的目标，而不是限速前的 QP 原始结果。

步骤 5：声明 ROS 标准消息依赖

在 xr_rm_teleop/package.xml 的 rclpy 依赖后增加：

  <exec_depend>sensor_msgs</exec_depend>

步骤 6：运行关节控制测试并确认通过

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：提交关节状态接口

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 改为接收适配器模式：

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，并新增：

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：运行新增测试并确认自动重新锚定测试失败

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 后解除重新使能要求

在参数读取区域保存：

        self._use_mock = self._bool_parameter("use_mock")

把 _make_adapter() 中的判断改为：

        if self._use_mock:
            return MockRealManAdapter(initial_joint_pose)

在 _handle_initial_pose_button() 中保持 Reset 前先锁存：

        self._grip_rearm_required = True

并只在 move_to_initial_pose()、read_joint_state() 和 _reset_joint_state() 全部成功后增加：

        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：运行完整关节与姿态控制测试

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 行为

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 增加：

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：运行配置测试并确认文件不存在

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：

# 双 RM75 MuJoCo 运动学显示参数。初始姿态和控制限制仍由 dual_arm_rm75.yaml 管理。
dual_arm_simulator:
  ros__parameters:
    render_rate_hz: 60.0

步骤 4：在同一生产文件中增加最小 ROS wrapper

在 dual_arm_simulator.py 增加导入：

from typing import Callable

import mujoco.viewer
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState

在 DualArmKinematicModel 后增加：

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 后增加：

    entry_points={
        "console_scripts": [
            "dual_arm_simulator = xr_rm_mujoco.dual_arm_simulator:main",
        ],
    },

步骤 6：运行 MuJoCo 测试并确认通过

运行：

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 与配置

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：

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 测试并确认缺少参数和校验函数

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() 后增加：

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() 读取：

    use_mujoco = _as_bool(
        LaunchConfiguration("use_mujoco").perform(context)
    )

在已有 arm 校验后调用：

    _validate_mujoco_mode(arm, use_mujoco)

在左右遥操作节点加入完成后追加：

    if use_mujoco:
        nodes.append(_mujoco_node())

在 generate_launch_description() 中 use_mock 后增加：

        # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
        DeclareLaunchArgument("use_mujoco", default_value="false"),

步骤 4：声明 bringup 对新包的运行依赖

在 xr_rm_bringup/package.xml 的 xr_rm_input 后增加：

  <exec_depend>xr_rm_mujoco</exec_depend>

步骤 5：运行 launch 测试并确认通过

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 集成

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 中做以下明确修改：

在“已完成”加入 MuJoCo 运动学显示；
在结构树加入 xr_rm_mujoco 和 dual_arm_mujoco.yaml；
在 launch 参数加入 use_mujoco；
增加下面两条命令，并明确第二条会连接真机：

# 无真机：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

记录关节反馈话题、目标话题、60 Hz viewer、90 Hz MuJoCo 状态输入和真机原始 200 Hz 反馈；
记录左 X/右 A Reset：Mock 立即复位且 Grip 保持时自动重锚，真机仍需松开 Grip；
明确 move_to_initial_pose_on_connect 继续保持 false。

步骤 2：运行局部测试

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：构建整个工作空间并检查安装资源

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

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 链路

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：检查安全配置未被改变

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

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

38 KiB Raw Blame History Unescape Escape

双臂 MuJoCo 运动学遥操作实施计划

执行约束

文件结构

任务一：建立 MuJoCo 双臂运动学模型

任务二：发布左右关节反馈与限速目标

任务三：让 Mock Reset 后立即重新锚定

任务四：增加 ROS MuJoCo viewer 节点和独立配置

任务五：接入统一 launch 并保持默认行为

任务六：更新文档并完成全量 Mock 验收

完成标准

38 KiB

Raw Blame History