acRealman_xr/docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md

RM75 Placo 单步 QP 逆解 Implementation Plan

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: 用 Placo 0.9.4 每控制周期执行一次 QP，把左右 RM75 的工具 TCP 目标转换成 7 个关节目标并通过 rm_movej_canfd 下发。

Architecture: 继续使用现有 single_arm_velocity_teleop 节点、RealManAdapter 和 arm_debug.launch.py。新增一个进程内 PlacoIkSolver，从首帧有效关节反馈初始化 robot.state.q[7:14]，固定虚拟基座，并在现有 TCP 安全限幅之后执行一步 QP；Adapter 的同一个 SDK 连接同时承担关节反馈、关节指令、安全停止和工具控制。

Tech Stack: Ubuntu 22.04、ROS2 Humble、Python 3.10、Placo 0.9.4、Pin 3.7.0、NumPy、睿尔曼 Python API2、pytest。

Repository rule: 未获得用户明确授权前不执行 git commit、git push 或真机命令。所有 Codex 运行验证均使用 use_mock:=true；真机步骤只写入检查清单，由用户执行。

---

文件结构

本次只触及以下职责边界：

• Create: xr_rm_teleop/models/rm75/RM75-B.urdf
• Create: xr_rm_teleop/models/rm75/meshes/base_link.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_1.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_2.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_3.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_4.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_5.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_6.STL
• Create: xr_rm_teleop/models/rm75/meshes/link_7.STL
• Create: xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py — Placo 模型、工具变换和一步 QP。
• Create: xr_rm_teleop/test/test_placo_transforms.py — 不依赖 Placo 的 TCP/法兰变换测试。
• Create: xr_rm_teleop/test/test_joint_control.py — 首帧反馈门控和 last-known-good 测试。
• Create: xr_rm_teleop/test/placo_ik_smoke.py — 使用 XR Python 的左右臂 45 周期数值验收。
• Modify: xr_rm_teleop/xr_rm_teleop/fun_peripheral.py — 让已加载的配置直接提供所选工具名和位姿。
• Modify: xr_rm_teleop/xr_rm_teleop/realman_adapter.py — 用关节反馈缓存和 rm_movej_canfd 替换笛卡尔接口。
• Modify: xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py — 保留现有 TCP 目标生成与安全限幅，接入反馈门控和 QP。
• Modify: xr_rm_teleop/test/test_initial_joint_pose.py — 覆盖 SDK 关节反馈与关节透传参数。
• Modify: xr_rm_teleop/test/test_orientation_control.py — 删除已移除的 mock 笛卡尔接口测试，其余姿态控制回归测试保持不变。
• Modify: xr_rm_teleop/setup.py — 安装 URDF 和网格，不声明或下载 Placo。
• Modify: xr_rm_bringup/launch/arm_debug.launch.py — 显式用 XR Python 启动遥操作节点并传入 URDF。
• Modify: xr_rm_bringup/config/left_arm_rm75.yaml
• Modify: xr_rm_bringup/config/right_arm_rm75.yaml
• Modify: xr_rm_bringup/config/dual_arm_rm75.yaml
• Modify: README.md — 更新控制链路、环境约束和左右臂独立验收步骤。

不修改 xr_rm_interfaces，不新增 ROS 消息，不新增 QP ROS 节点，不修改 launcher_ui.py 的进程拓扑。

Task 1: 导入 RM75 模型并实现工具变换

Files:

• Create: xr_rm_teleop/models/rm75/RM75-B.urdf

• Create: xr_rm_teleop/models/rm75/meshes/*.STL

• Create: xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

• Create: xr_rm_teleop/test/test_placo_transforms.py

• Modify: xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

• Step 1: 从用户上传包复制且只复制模型资源

从工作空间根目录执行：

cd /home/robot/WS_xr
model_tmp_dir=$(mktemp -d /tmp/rm75-model.XXXXXX)
unzip -q /home/robot/XRoboToolkit/Resource/IK_qp-class_version.zip \
  'ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf' \
  'ik_qp/kine_ctrl/urdf_rm75/meshes/*' \
  -d "$model_tmp_dir"
mkdir -p src/xr_rm_teleop/models/rm75/meshes
cp "$model_tmp_dir/ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf" \
  src/xr_rm_teleop/models/rm75/RM75-B.urdf
cp "$model_tmp_dir/ik_qp/kine_ctrl/urdf_rm75/meshes/"*.STL \
  src/xr_rm_teleop/models/rm75/meshes/

检查只导入一个 URDF 和八个网格，没有导入上传包的 Pinocchio/OSQP 代码：

find src/xr_rm_teleop/models/rm75 -type f | sort

Expected: RM75-B.urdf、base_link.STL、link_1.STL 至 link_7.STL，共 9 个文件。

• Step 2: 先写 TCP/法兰变换失败测试

创建 src/xr_rm_teleop/test/test_placo_transforms.py：

import math

import numpy as np
import pytest

from xr_rm_teleop.placo_ik_solver import (
    _arm_pose_to_transform,
    _tool_pose_to_transform,
    _transform_to_arm_pose,
)
from xr_rm_teleop.realman_adapter import ArmPose


def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]

    base_to_flange = _arm_pose_to_transform(flange_pose)
    flange_to_tool = _tool_pose_to_transform(tool_pose)
    base_to_tool = base_to_flange @ flange_to_tool
    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)

    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
    assert recovered_flange == pytest.approx(base_to_flange)


def test_transform_to_arm_pose_roundtrip() -> None:
    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)

    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))

    assert actual.xyz() == pytest.approx(expected.xyz())
    assert actual.rpy() == pytest.approx(expected.rpy())

• Step 3: 运行测试并确认它因模块尚不存在而失败

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_placo_transforms.py

Expected: FAIL，错误包含 No module named 'xr_rm_teleop.placo_ik_solver'。

• Step 4: 给外设配置增加所选工具的单一来源

在 PeripheralConfig 中加入：

    @property
    def tool_name(self) -> str:
        return list(self.tools_in_ee)[self.scissorgripper]

    @property
    def tool_pose(self) -> list[float]:
        return list(self.tools_in_ee[self.tool_name][0])

把 RealManAdapter.configure_peripheral() 中原来的工具名选择替换为：

        tool_name = config.tool_name

工具位姿和控制器工具帧由同一个 PeripheralConfig 提供，避免左右臂工具索引在两个位置各自解析。

• Step 5: 实现不在系统 Python 导入 Placo 的变换和求解器骨架

创建 src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py，模块顶层只导入标准库和 NumPy；placo 必须在构造函数内导入：

"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""

from __future__ import annotations

import math
from importlib.metadata import PackageNotFoundError, version
from pathlib import Path

import numpy as np

from .realman_adapter import ArmPose


EXPECTED_PLACO_VERSION = "0.9.4"
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
RM75_Q_SLICE = slice(7, 14)


def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
    cr, sr = math.cos(roll), math.sin(roll)
    cp, sp = math.cos(pitch), math.sin(pitch)
    cy, sy = math.cos(yaw), math.sin(yaw)
    return np.array(
        [
            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
            [-sp, cp * sr, cp * cr],
        ],
        dtype=float,
    )


def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
    pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
    if abs(math.cos(pitch)) > 1e-9:
        roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
        yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
    else:
        roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
        yaw = 0.0
    return roll, pitch, yaw


def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
    transform = np.eye(4)
    transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
    transform[:3, 3] = pose.xyz()
    return transform


def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
    values = np.asarray(tool_pose, dtype=float)
    if values.shape != (7,) or not np.isfinite(values).all():
        raise ValueError("tool pose must contain 7 finite values")
    x, y, z, qx, qy, qz, qw = values
    norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
    if norm <= 1e-9:
        raise ValueError("tool quaternion norm must be positive")
    qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
    transform = np.eye(4)
    transform[:3, :3] = np.array(
        [
            [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
            [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
            [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
        ]
    )
    transform[:3, 3] = [x, y, z]
    return transform


def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
    roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
    return ArmPose(
        float(transform[0, 3]),
        float(transform[1, 3]),
        float(transform[2, 3]),
        roll,
        pitch,
        yaw,
    )

• Step 6: 运行纯变换测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_placo_transforms.py

Expected: 2 passed。系统 Python 不需要也不应能导入 Placo。

• Step 7: 检查 Task 1 变更范围

git diff -- \
  src/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py \
  src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
  src/xr_rm_teleop/test/test_placo_transforms.py
git status --short src/xr_rm_teleop/models/rm75

Expected: 只包含工具选择属性、变换模块、测试和模型资源。不要提交；等待用户明确授权。

Task 2: 实现 Placo 0.9.4 单步求解

Files:

• Modify: xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

• Create: xr_rm_teleop/test/placo_ik_smoke.py

• Step 1: 写 XR Python 左右臂数值验收脚本

创建 src/xr_rm_teleop/test/placo_ik_smoke.py：

from __future__ import annotations

import math
import sys
import time
from pathlib import Path

import numpy as np

from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
from xr_rm_teleop.realman_adapter import ArmPose


CASES = {
    "left": (
        [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
        [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
    ),
    "right": (
        [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
        [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
    ),
}


def angle_error(actual: list[float], target: list[float]) -> float:
    deltas = [
        math.atan2(math.sin(a - b), math.cos(a - b))
        for a, b in zip(actual, target)
    ]
    return math.sqrt(sum(value * value for value in deltas))


def main() -> None:
    urdf_path = Path(sys.argv[1]).resolve()
    for arm, (joint_degrees, tool_pose) in CASES.items():
        solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
        joints = np.deg2rad(joint_degrees).tolist()
        current = solver.update_joint_state(joints)
        target = ArmPose(
            current.x + 0.01,
            current.y,
            current.z,
            current.rx,
            current.ry,
            current.rz + 0.05,
        )

        solve_durations = []
        for _ in range(45):
            solver.update_joint_state(joints)
            started_at = time.perf_counter()
            joints = solver.solve(target)
            solve_durations.append(time.perf_counter() - started_at)

        actual = solver.update_joint_state(joints)
        position_error = np.linalg.norm(
            np.asarray(actual.xyz()) - np.asarray(target.xyz())
        )
        orientation_error = angle_error(actual.rpy(), target.rpy())
        assert len(joints) == 7
        assert np.isfinite(joints).all()
        assert np.allclose(
            solver.base_configuration,
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
        )
        assert position_error <= 0.005
        assert orientation_error <= math.radians(2.0)
        print(
            f"{arm}: position_error={position_error:.6f}m, "
            f"orientation_error={math.degrees(orientation_error):.3f}deg, "
            f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
            f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
            f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
        )


if __name__ == "__main__":
    main()

该脚本不是 pytest 收集文件；它由已有 XR Python 直接运行，全部断言只使用 Python 和 NumPy，不向该 Conda 环境安装 pytest。

• Step 2: 运行脚本并确认求解器类尚不存在

cd /home/robot/WS_xr
PYTHONPATH=src/xr_rm_teleop \
  /home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/placo_ik_smoke.py \
  src/xr_rm_teleop/models/rm75/RM75-B.urdf

Expected: FAIL，错误包含 cannot import name 'PlacoIkSolver'。

• Step 3: 实现固定版本、固定基座和 q[7:14] 映射

在 placo_ik_solver.py 追加：

class PlacoIkSolver:
    def __init__(
        self,
        urdf_path: str,
        tool_pose: list[float],
        dt: float,
    ) -> None:
        if dt <= 0.0:
            raise ValueError("dt must be positive")
        try:
            installed_version = version("placo")
            import placo
        except (ImportError, PackageNotFoundError) as exc:
            raise RuntimeError(
                "Placo 0.9.4 must come from "
                "/home/robot/miniconda3/envs/xr"
            ) from exc
        if installed_version != EXPECTED_PLACO_VERSION:
            raise RuntimeError(
                f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}"
            )

        model_path = Path(urdf_path).expanduser().resolve()
        if not model_path.is_file():
            raise FileNotFoundError(f"RM75 URDF not found: {model_path}")

        self._dt = dt
        self._robot = placo.RobotWrapper(str(model_path))
        if self._robot.state.q.shape != (14,):
            raise RuntimeError(
                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
            )
        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
            raise RuntimeError(
                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
            )
        offsets = [
            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
        ]
        if offsets != list(range(7, 14)):
            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")

        self._joint_limits = np.asarray(
            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
        )
        velocity_offsets = [
            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
        ]
        self._velocity_limits = np.asarray(
            [self._robot.model.velocityLimit[index] for index in velocity_offsets]
        )
        self._tool_transform = _tool_pose_to_transform(tool_pose)
        self._tool_inverse = np.linalg.inv(self._tool_transform)
        self._actual_joints: np.ndarray | None = None

        self._solver = placo.KinematicsSolver(self._robot)
        self._solver.dt = dt
        self._solver.mask_fbase(True)
        self._solver.enable_velocity_limits(True)
        self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
        self._frame_task.configure("rm75_frame", "soft", 1.0)
        manipulability = self._solver.add_manipulability_task(
            "link_7", "both", 1.0
        )
        manipulability.configure("rm75_manipulability", "soft", 5e-2)
        self._solver.add_kinetic_energy_regularization_task(1e-6)

    @property
    def base_configuration(self) -> list[float]:
        return self._robot.state.q[:7].tolist()

    def update_joint_state(self, joints: list[float]) -> ArmPose:
        values = np.asarray(joints, dtype=float)
        if values.shape != (7,) or not np.isfinite(values).all():
            raise ValueError("joint state must contain 7 finite values")
        is_first_feedback = self._actual_joints is None
        self._actual_joints = values.copy()
        self._robot.state.q[RM75_Q_SLICE] = values
        self._robot.update_kinematics()
        base_to_flange = self._robot.get_T_world_frame("link_7")
        if is_first_feedback:
            self._frame_task.T_world_frame = base_to_flange.copy()
        base_to_tool = base_to_flange @ self._tool_transform
        return _transform_to_arm_pose(base_to_tool)

    def solve(self, target_tool_pose: ArmPose) -> list[float]:
        if self._actual_joints is None:
            raise RuntimeError("joint state must be initialized before QP solve")
        self._frame_task.T_world_frame = (
            _arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
        )
        self._solver.solve(True)
        result = np.asarray(
            self._robot.state.q[RM75_Q_SLICE],
            dtype=float,
        ).copy()
        self._validate_result(result)
        return result.tolist()

    def _validate_result(self, result: np.ndarray) -> None:
        if result.shape != (7,) or not np.isfinite(result).all():
            raise ValueError("QP result must contain 7 finite values")
        lower = self._joint_limits[:, 0]
        upper = self._joint_limits[:, 1]
        if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
            raise ValueError("QP result violates RM75 joint position limits")
        max_step = self._velocity_limits * self._dt + 1e-9
        if np.any(np.abs(result - self._actual_joints) > max_step):
            raise ValueError("QP result violates RM75 one-cycle velocity limits")

不要调用 neutral 全零求解。构造函数只建立任务；第一次 solve() 必须发生在 update_joint_state() 之后。

• Step 4: 增加求解结果校验测试

在 test_placo_transforms.py 的 solver import 中加入 PlacoIkSolver，并追加：

def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
    solver = object.__new__(PlacoIkSolver)
    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
    solver._velocity_limits = np.ones(7)
    solver._dt = 0.1
    solver._actual_joints = np.zeros(7)

    with pytest.raises(ValueError, match="finite"):
        solver._validate_result(np.full(7, np.nan))
    with pytest.raises(ValueError, match="position"):
        solver._validate_result(np.full(7, 2.0))
    with pytest.raises(ValueError, match="velocity"):
        solver._validate_result(np.full(7, 0.2))

运行：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_placo_transforms.py

Expected: 3 passed，且系统 Python 没有导入 Placo。

• Step 5: 运行左右臂 45 周期 QP 验收

cd /home/robot/WS_xr
PYTHONPATH=src/xr_rm_teleop \
  /home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/placo_ik_smoke.py \
  src/xr_rm_teleop/models/rm75/RM75-B.urdf

Expected:

left: position_error=<不大于0.005>m, orientation_error=<不大于2.0>deg
right: position_error=<不大于0.005>m, orientation_error=<不大于2.0>deg

允许 Placo 打印 neutral 自碰撞警告；不得出现 QP NaN，基座必须保持 [0, 0, 0, 0, 0, 0, 1]。

• Step 6: 检查 Task 2 变更，不提交

git diff -- \
  src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
  src/xr_rm_teleop/test/placo_ik_smoke.py

Expected: 一个求解器类和一个左右臂数值验收脚本，不包含 Placo 安装命令、可视化、OSQP 或 XRoboToolkit import。

Task 3: 把 Adapter 改成单连接关节反馈与关节透传

Files:

• Modify: xr_rm_teleop/xr_rm_teleop/realman_adapter.py

• Modify: xr_rm_teleop/test/test_initial_joint_pose.py

• Modify: xr_rm_teleop/test/test_orientation_control.py

• Step 1: 先写 SDK 关节边界失败测试

在 test_initial_joint_pose.py 保留现有初始化 rm_movej 测试，并追加：

import math

import pytest

from xr_rm_teleop.realman_adapter import MockRealManAdapter


def test_joint_feedback_is_cached_in_radians() -> None:
    class FakeArm:
        def rm_get_joint_degree(self):
            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]

    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
    adapter._arm = FakeArm()

    adapter._read_joint_state_once()
    snapshot = adapter.get_latest_joint_state()

    assert snapshot is not None
    assert snapshot.positions == pytest.approx(
        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
    )


def test_joint_target_uses_movej_canfd_in_degrees() -> None:
    class FakeArm:
        def __init__(self) -> None:
            self.calls = []

        def rm_movej_canfd(self, *args):
            self.calls.append(args)
            return 0

    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
    adapter._arm = FakeArm()
    target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]

    adapter.send_joint_target(target, follow=False)

    assert adapter._arm.calls == [
        ([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0], False, 0, 2, 0)
    ]


def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])

    adapter.connect()
    snapshot = adapter.get_latest_joint_state()

    assert snapshot is not None
    assert snapshot.positions == pytest.approx(
        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
    )

从 test_orientation_control.py 删除 test_mock_adapter_uses_shortest_angular_velocity()，并删除不再使用的 MockRealManAdapter import。其余姿态测试不改。

• Step 2: 运行测试并确认旧 Adapter API 不满足测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_initial_joint_pose.py

Expected: FAIL，至少包含 get_latest_joint_state、send_joint_target 或构造参数不匹配。

• Step 3: 用线程安全反馈快照替换笛卡尔 mock

在 realman_adapter.py：

import threading
import time

加入：

@dataclass(frozen=True)
class JointStateSnapshot:
    positions: list[float]
    received_at: float

将 MockRealManAdapter 替换成：

class MockRealManAdapter:
    def __init__(self, initial_joint_degrees: list[float]) -> None:
        if len(initial_joint_degrees) != 7:
            raise ValueError("initial joint pose must contain 7 values")
        self._joint_positions = [
            math.radians(value) for value in initial_joint_degrees
        ]
        self.last_joint_target: list[float] | None = None
        self.last_tool_open: bool | None = None

    def connect(self) -> None:
        return

    def get_latest_joint_state(self) -> JointStateSnapshot:
        return JointStateSnapshot(
            list(self._joint_positions),
            time.monotonic(),
        )

    def send_joint_target(self, joints: list[float], follow: bool) -> None:
        del follow
        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
            raise ValueError("joint target must contain 7 finite values")
        self._joint_positions = list(joints)
        self.last_joint_target = list(joints)

    def stop(self) -> None:
        return

    def close(self) -> None:
        self.stop()

    def configure_peripheral(self, config, peripheral_arm: str) -> None:
        del config, peripheral_arm

    def set_tool_enabled(self, open_tool: bool) -> None:
        self.last_tool_open = open_tool

删除 Adapter 内不再使用的 _angle_delta() 和笛卡尔发送/读取方法；保留 ArmPose，因为遥操作和 Placo 仍用它表示工具 TCP。Number 继续用于 SDK 反馈数值校验。

• Step 4: 在 RealManAdapter 的同一连接上加入反馈缓存

把构造参数中的 frame_type 替换为：

        feedback_period: float,

在构造函数末尾加入：

        if feedback_period <= 0.0:
            raise ValueError("feedback_period must be positive")
        self._feedback_period = feedback_period
        self._joint_state_lock = threading.Lock()
        self._latest_joint_state: JointStateSnapshot | None = None
        self._feedback_stop = threading.Event()
        self._feedback_thread: threading.Thread | None = None
        self._feedback_fault_logged = False

连接成功日志中的命令改成 rm_movej_canfd，在安全配置和可选初始移动之后启动反馈线程：

        self._feedback_stop.clear()
        self._feedback_thread = threading.Thread(
            target=self._feedback_loop,
            name=f"rm75_feedback_{self._robot_ip}",
            daemon=True,
        )
        self._feedback_thread.start()

加入以下方法：

    def _feedback_loop(self) -> None:
        while not self._feedback_stop.is_set():
            try:
                self._read_joint_state_once()
                self._feedback_fault_logged = False
            except Exception as exc:
                if not self._feedback_fault_logged:
                    self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
                    self._feedback_fault_logged = True
            self._feedback_stop.wait(self._feedback_period)

    def _read_joint_state_once(self) -> None:
        self._require_arm()
        result = self._arm.rm_get_joint_degree()
        self._check_return(result, "rm_get_joint_degree")
        if not isinstance(result, tuple) or len(result) < 2:
            raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
        degrees = result[1]
        if (
            not isinstance(degrees, (list, tuple))
            or len(degrees) != 7
            or not all(isinstance(value, Number) for value in degrees)
        ):
            raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
        positions = [math.radians(float(value)) for value in degrees]
        if not all(math.isfinite(value) for value in positions):
            raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
        snapshot = JointStateSnapshot(positions, time.monotonic())
        with self._joint_state_lock:
            self._latest_joint_state = snapshot

    def get_latest_joint_state(self) -> JointStateSnapshot | None:
        with self._joint_state_lock:
            if self._latest_joint_state is None:
                return None
            return JointStateSnapshot(
                list(self._latest_joint_state.positions),
                self._latest_joint_state.received_at,
            )

    def send_joint_target(self, joints: list[float], follow: bool) -> None:
        self._require_arm()
        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
            raise ValueError("joint target must contain 7 finite values")
        degrees = [math.degrees(value) for value in joints]
        ret = self._arm.rm_movej_canfd(
            degrees,
            follow,
            0,
            self._canfd_trajectory_mode,
            self._canfd_radio,
        )
        self._check_return(ret, "rm_movej_canfd")

Number 仍用于 SDK 反馈校验，因此保留 from numbers import Number。

• Step 5: 让工具配置复用已加载的 PeripheralConfig

把真机和 mock 的签名统一为：

    def configure_peripheral(self, config, peripheral_arm: str) -> None:

真机实现不再读取 YAML，直接使用：

        self._scissorgripper = config.scissorgripper
        tool_name = config.tool_name
        peripheral_cfg(
            self._arm,
            config.scissorgripper,
            config.tools_in_ee,
            set_initial_tool_state=config.set_initial_tool_state,
        )

这样 Placo 与控制器工具帧使用同一次 YAML 解析结果。

• Step 6: 安全停止后结束反馈线程，再删除唯一连接

将 close() 改为：

    def close(self) -> None:
        if self._arm is None:
            return
        self.stop()
        self._feedback_stop.set()
        if self._feedback_thread is not None:
            self._feedback_thread.join(timeout=3.0)
            if self._feedback_thread.is_alive():
                self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
            self._feedback_thread = None
        try:
            self._arm.rm_delete_robot_arm()
        finally:
            self._arm = None

不得创建第二个 RoboticArm 或再次调用 rm_create_robot_arm。

• Step 7: 运行 Adapter 与姿态回归测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_initial_joint_pose.py \
  src/xr_rm_teleop/test/test_orientation_control.py

Expected: 全部 PASS；mock 测试过程中不需要 Robotic_Arm。

• Step 8: 静态确认只有一个连接创建点

rg -n "rm_create_robot_arm" src/xr_rm_teleop

Expected: 只有 realman_adapter.py 的 connect() 中一处调用。

Task 4: 在单臂节点中加入首帧反馈门控和 XR 失败策略

Files:

• Create: xr_rm_teleop/test/test_joint_control.py

• Modify: xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

• Step 1: 写首帧门控和 last-known-good 失败测试

创建 src/xr_rm_teleop/test/test_joint_control.py：

import time
from types import SimpleNamespace

import pytest

from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop


class FakeLogger:
    def warn(self, *args, **kwargs):
        del args, kwargs


def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._command_timeout_sec = 0.12
    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)

    assert teleop._fresh_joint_state() is None

    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.0] * 7,
            time.monotonic() - 1.0,
        )
    )
    assert teleop._fresh_joint_state() is None


def test_stale_feedback_stops_before_qp_solve() -> None:
    class SolverThatMustNotRun:
        def solve(self, target):
            del target
            raise AssertionError("QP must not run with stale feedback")

    stopped = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.0] * 7,
            time.monotonic() - 1.0,
        )
    )
    teleop._ik_solver = SolverThatMustNotRun()
    teleop._command_timeout_sec = 0.12
    teleop._joint_feedback_ready = True
    teleop._arm_name = "right_rm75"
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: object())
    teleop.get_logger = lambda: FakeLogger()
    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)

    teleop._control_tick()

    assert stopped == [True]


def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
    class FakeSolver:
        def __init__(self) -> None:
            self.solve_calls = 0

        def update_joint_state(self, joints):
            assert joints == [0.1] * 7
            return ArmPose(0.3, 0.0, 0.2)

        def solve(self, target):
            del target
            self.solve_calls += 1
            return [0.2] * 7

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = FakeSolver()
    teleop._active = False
    teleop._last_valid_joint_target = None
    teleop._last_current_pose = None

    pose = teleop._sync_joint_feedback(
        JointStateSnapshot([0.1] * 7, time.monotonic())
    )

    assert pose == ArmPose(0.3, 0.0, 0.2)
    assert teleop._last_valid_joint_target == [0.1] * 7
    assert teleop._ik_solver.solve_calls == 0


def test_qp_failure_returns_last_known_good_target() -> None:
    class FailingSolver:
        def solve(self, target):
            del target
            raise RuntimeError("NaN in QP solution")

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = FailingSolver()
    teleop._last_valid_joint_target = [0.1] * 7
    teleop._arm_name = "right_rm75"
    teleop.get_logger = lambda: FakeLogger()

    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))

    assert target == pytest.approx([0.1] * 7)
    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)


def test_qp_success_updates_last_known_good_target() -> None:
    class SuccessfulSolver:
        def solve(self, target):
            del target
            return [0.2] * 7

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = SuccessfulSolver()
    teleop._last_valid_joint_target = [0.1] * 7
    teleop._arm_name = "left_rm75"
    teleop.get_logger = lambda: FakeLogger()

    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))

    assert target == pytest.approx([0.2] * 7)
    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)

• Step 2: 运行测试并确认节点尚无这些方法

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_joint_control.py

Expected: FAIL，错误包含 _fresh_joint_state、_sync_joint_feedback 或 _solve_joint_target 不存在。

• Step 3: 让节点启动时先解析工具并创建 Placo，再连接 Adapter

在 import 中加入：

import time

from .fun_peripheral import load_peripheral_config
from .placo_ik_solver import PlacoIkSolver
from .realman_adapter import (
    ArmPose,
    JointStateSnapshot,
    MockRealManAdapter,
    RealManAdapter,
)

声明新参数：

        self.declare_parameter("robot_urdf_path", "")

删除已失效的参数声明和成员读取：

current_pose_poll_hz
mock_initial_pose
frame_type

初始化字段加入：

        self._last_valid_joint_target: list[float] | None = None
        self._joint_feedback_ready = False

把 Adapter 创建顺序替换为：

        peripheral_arm = self._peripheral_arm_name()
        config_file = str(self.get_parameter("peripheral_config_file").value)
        self._peripheral_config = load_peripheral_config(
            config_file,
            peripheral_arm,
        )
        self._ik_solver = PlacoIkSolver(
            str(self.get_parameter("robot_urdf_path").value),
            self._peripheral_config.tool_pose,
            self._dt,
        )
        self._adapter = self._make_adapter()
        self._adapter.connect()
        self._setup_tool_control()

这样 Placo 版本、URDF、关节映射和工具位姿在建立真机连接前完成校验。

• Step 4: 让 mock 和真机 Adapter 都使用初始关节配置

将 _make_adapter() 改为：

    def _make_adapter(self):
        initial_joint_pose = self._float_list_parameter(
            "initial_joint_pose",
            7,
        )
        if self._bool_parameter("use_mock"):
            return MockRealManAdapter(initial_joint_pose)

        return RealManAdapter(
            robot_ip=self.get_parameter("robot_ip").value,
            robot_port=int(self.get_parameter("robot_port").value),
            avoid_singularity=int(
                self.get_parameter("avoid_singularity").value
            ),
            feedback_period=self._dt,
            logger=self.get_logger(),
            configure_safety_limits=self._bool_parameter(
                "configure_safety_limits"
            ),
            max_line_speed=float(self.get_parameter("max_line_speed").value),
            max_angular_speed=float(
                self.get_parameter("max_angular_speed").value
            ),
            max_line_acc=float(self.get_parameter("max_line_acc").value),
            max_angular_acc=float(
                self.get_parameter("max_angular_acc").value
            ),
            joint_max_speed=float(
                self.get_parameter("joint_max_speed").value
            ),
            joint_max_acc=float(self.get_parameter("joint_max_acc").value),
            move_to_initial_pose_on_connect=self._bool_parameter(
                "move_to_initial_pose_on_connect"
            ),
            initial_joint_pose=initial_joint_pose,
            init_move_speed=int(self.get_parameter("init_move_speed").value),
            canfd_trajectory_mode=int(
                self.get_parameter("canfd_trajectory_mode").value
            ),
            canfd_radio=int(self.get_parameter("canfd_radio").value),
        )

• Step 5: 工具坐标配置与工具开合解耦

在 _setup_tool_control() 开头先执行控制器工具配置：

        peripheral_arm = self._peripheral_arm_name()
        if self._bool_parameter("configure_peripheral_on_connect"):
            self._adapter.configure_peripheral(
                self._peripheral_config,
                peripheral_arm,
            )

然后再判断：

        if not self._enable_tool_control:

删除该函数中重复读取 peripheral_config_file 和重复调用旧 configure_peripheral(config_file, peripheral_arm) 的代码。

• Step 6: 实现反馈时效、首帧初始化和 QP 失败回退

在节点类中加入：

    def _fresh_joint_state(self) -> JointStateSnapshot | None:
        snapshot = self._adapter.get_latest_joint_state()
        if snapshot is None:
            return None
        age = time.monotonic() - snapshot.received_at
        if age < 0.0 or age > self._command_timeout_sec:
            return None
        if (
            len(snapshot.positions) != 7
            or not all(math.isfinite(value) for value in snapshot.positions)
        ):
            return None
        return snapshot

    def _sync_joint_feedback(
        self,
        snapshot: JointStateSnapshot,
    ) -> ArmPose:
        current_pose = self._ik_solver.update_joint_state(
            snapshot.positions
        )
        self._last_current_pose = current_pose
        if not self._active or self._last_valid_joint_target is None:
            self._last_valid_joint_target = list(snapshot.positions)
        return current_pose

    def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
        if self._last_valid_joint_target is None:
            raise RuntimeError("valid joint feedback has not been initialized")
        try:
            result = self._ik_solver.solve(target_pose)
        except Exception as exc:
            self.get_logger().warn(
                f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                throttle_duration_sec=1.0,
            )
            return list(self._last_valid_joint_target)
        self._last_valid_joint_target = list(result)
        return list(result)

    def _send_joint_target(self, joints: list[float]) -> bool:
        try:
            self._adapter.send_joint_target(joints, self._follow)
        except Exception as exc:
            self.get_logger().error(
                f"{self._arm_name} 发送关节透传命令失败：{exc}",
                throttle_duration_sec=1.0,
            )
            self._safe_stop(reset_active=True)
            return False
        return True

• Step 7: 在每个控制周期最前面执行反馈门控

_control_tick() 在读取 XR 消息前加入：

        snapshot = self._fresh_joint_state()
        if snapshot is None:
            if self._joint_feedback_ready:
                self.get_logger().warn(
                    f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
                    throttle_duration_sec=1.0,
                )
            self._joint_feedback_ready = False
            self._safe_stop(reset_active=True)
            return
        try:
            current_pose = self._sync_joint_feedback(snapshot)
        except Exception as exc:
            self.get_logger().error(
                f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
                throttle_duration_sec=1.0,
            )
            self._joint_feedback_ready = False
            self._safe_stop(reset_active=True)
            return
        if not self._joint_feedback_ready:
            self.get_logger().info(
                f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
            )
            self._joint_feedback_ready = True

当 grip 首次激活时，把当前 current_pose 传给 _enter_active_control()；该函数不再调用 Adapter 读取 TCP：

            self._enter_active_control(
                controller_now,
                controller_quat,
                current_pose,
                now,
            )

函数签名改为：

    def _enter_active_control(
        self,
        controller_now: list[float],
        controller_quat: tuple[float, float, float, float],
        robot_pose: ArmPose,
        now: Time,
    ) -> None:

删除该函数原来的 try: robot_pose = self._read_current_pose_for_control(now) 块。

• Step 8: 用一步 QP 和关节透传替换笛卡尔下发

删除 _maybe_refresh_current_pose(now) 调用。保留从 raw_target_xyz 到 target_pose 的全部死区、滤波、工作空间/圆柱、线速度和角速度限制。

把：

        if self._send_cartesian_target(target_pose):

替换为：

        joint_target = self._solve_joint_target(target_pose)
        if self._send_joint_target(joint_target):

删除以下旧方法：

_read_current_pose_for_control
_maybe_refresh_current_pose
_send_cartesian_target

每个有效且激活的控制周期只能有一次 self._ik_solver.solve(target_pose)。

• Step 9: 运行节点行为和姿态回归测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_joint_control.py \
  src/xr_rm_teleop/test/test_orientation_control.py

Expected: 全部 PASS。

• Step 10: 静态确认旧笛卡尔运动链路已移除

rg -n "rm_movep_canfd|send_cartesian_target|get_current_pose" \
  src/xr_rm_teleop/xr_rm_teleop

Expected: 遥操作包的运行代码中无匹配。

Task 5: 固定 XR Python 启动并安装模型资源

Files:

• Modify: xr_rm_teleop/setup.py

• Modify: xr_rm_bringup/launch/arm_debug.launch.py

• Modify: xr_rm_bringup/config/left_arm_rm75.yaml

• Modify: xr_rm_bringup/config/right_arm_rm75.yaml

• Modify: xr_rm_bringup/config/dual_arm_rm75.yaml

• Step 1: 在 setup.py 中只安装模型，不声明 pip Placo 依赖

加入：

from glob import glob

把 data_files 扩展为：

    data_files=[
        (
            "share/ament_index/resource_index/packages",
            [f"resource/{package_name}"],
        ),
        (f"share/{package_name}", ["package.xml"]),
        (
            f"share/{package_name}/models/rm75",
            ["models/rm75/RM75-B.urdf"],
        ),
        (
            f"share/{package_name}/models/rm75/meshes",
            glob("models/rm75/meshes/*.STL"),
        ),
    ],

保持：

    install_requires=["setuptools"],

不得加入 placo、pin、eigenpy 或 numpy，以免 colcon 使用系统 Python 时触发安装或升级。

• Step 2: launch 固定并验证 XR Python

在 arm_debug.launch.py 加入：

from pathlib import Path

定义：

XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"

在 _launch_setup() 最前面校验：

    if not Path(XR_PYTHON).is_file():
        raise RuntimeError(
            f"XR Python not found: {XR_PYTHON}; "
            "Placo 0.9.4 must not be installed globally"
        )

给三个 single_arm_velocity_teleop Node 对象都加入：

        prefix=[XR_PYTHON],

不要给 udp_controller_receiver 加 prefix；它继续使用系统 ROS2 Python。

• Step 3: launch 向每个遥操作节点传安装后的 URDF

给三个遥操作 Node 的参数字典都加入：

                "robot_urdf_path": PathJoinSubstitution([
                    FindPackageShare("xr_rm_teleop"),
                    "models",
                    "rm75",
                    "RM75-B.urdf",
                ]),

删除 launch 中已经失效的 frame_type 参数传递、函数参数和对应的 DeclareLaunchArgument。

将控制周期注释更新为：

        # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。

• Step 4: 同步左、右、双臂 YAML

三个 YAML 都删除：

current_pose_poll_hz
mock_initial_pose
frame_type

保留每侧原有：

initial_joint_pose
workspace_min / workspace_max
cyl_radius_limit
max_linear_speed / max_orientation_speed
configure_safety_limits: true

把左右单臂 YAML 都改为：

    move_to_initial_pose_on_connect: false

双臂文件原有左右两个 false 保持不变。把 rm_movep_canfd 注释改为 “Placo 单步 QP + rm_movej_canfd 关节透传”。

• Step 5: 从工作空间根目录构建

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install

Expected: build summary 无失败包。

• Step 6: 检查模型和 launch 安装结果

test -f install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/RM75-B.urdf
test -f install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/meshes/link_7.STL
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py --show-args

Expected: 两个 test 返回 0；launch 参数列表正常显示，不包含 frame_type。

• Step 7: 使用安装模型再次运行左右臂数值验收

cd /home/robot/WS_xr
PYTHONPATH=src/xr_rm_teleop \
  /home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/placo_ik_smoke.py \
  install/xr_rm_teleop/share/xr_rm_teleop/models/rm75/RM75-B.urdf

Expected: 左、右均满足 <=5 mm 和 <=2°。

Task 6: 更新文档并完成 mock 验证

Files:

• Modify: README.md

• Step 1: 更新 README 控制链路

把项目首页链路改为：

PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> TCP 安全限幅与工具/法兰目标换算
  -> Placo 0.9.4 每周期一步 QP
  -> rm_movej_canfd 发送 7 个 RM75 关节目标

明确说明：

遥操作节点收到首帧有效实际关节反馈前不调用 QP、不发送运动指令。
QP 失败时沿用上一组有效关节目标；XR 超时、反馈过期、Grip 松开或
SDK 发送失败时执行慢停止。

• Step 2: 更新环境说明

在“环境准备”加入：

Placo 固定复用 /home/robot/miniconda3/envs/xr：
- Python 3.10
- Placo 0.9.4
- Pin 3.7.0
- NumPy 2.2.6

arm_debug.launch.py 只对 single_arm_velocity_teleop 使用该 Python；
ros2、colcon 和 udp_controller_receiver 仍使用系统 Python。
禁止 pip --user、sudo pip 或系统级安装/升级 Placo、Pin、EigenPy 和 NumPy。

加入只读检查命令：

/home/robot/miniconda3/envs/xr/bin/python -c \
  "import importlib.metadata; print(importlib.metadata.version('placo'))"

Expected: 0.9.4。

• Step 3: 更新参数和真机安全说明

删除 README 对以下旧参数的描述：

frame_type
current_pose_poll_hz
mock_initial_pose

把 follow 描述改成传给 rm_movej_canfd。把单臂自动回初始位姿说明改成：

左臂、右臂和双臂默认都不会自动执行 movej(initial_joint_pose)。
只有用户在清空工作区并确认急停后显式传
move_to_initial_pose_on_connect:=true 才允许自动回初始位姿。

说明 peripherals_rm75.yaml 中左臂 minisci +0.19 m、右臂 omnipic +0.16 m 同时用于 RealMan 工具配置和 Placo TCP/法兰换算。

• Step 4: 运行完整自动测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
pytest -q \
  src/xr_rm_teleop/test/test_orientation_control.py \
  src/xr_rm_teleop/test/test_initial_joint_pose.py \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  src/xr_rm_teleop/test/test_joint_control.py

Expected: 全部 PASS。

• Step 5: 分别启动左臂和右臂 mock

左臂：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
timeout --signal=INT 8s \
  ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=left use_mock:=true

右臂：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
timeout --signal=INT 8s \
  ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=true

Expected: 两侧分别打印 Placo 0.9.4 启动、首帧有效关节反馈和节点启动日志； 没有 ModuleNotFoundError、QP NaN 或厂商 SDK import 错误。timeout 主动结束 launch 时退出码可以是 124。

• Step 6: 最终静态安全检查

cd /home/robot/WS_xr
rg -n "rm_movep_canfd|send_cartesian_target" \
  src/xr_rm_teleop src/xr_rm_bringup
rg -n "rm_create_robot_arm" src/xr_rm_teleop
rg -n "_ik_solver\\.solve" \
  src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
rg -n "configure_safety_limits: false|move_to_initial_pose_on_connect: true" \
  src/xr_rm_bringup/config
git diff --check
git status --short

Expected:

• 旧笛卡尔运动链路无匹配。

• rm_create_robot_arm 只有一个实现调用点。

• _ik_solver.solve 在控制节点中只有一个调用点。

• 安全配置中没有默认关闭安全限位或默认自动回初始位姿。

• git diff --check 无空白错误。

• 原有用户改动 D openspec/config.yaml 仍保持原样，未被本任务触碰。

• Step 7: 给用户提供左右臂各自的真机人工验收清单

只提供命令，不由 Codex 执行：

ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=left use_mock:=false \
  move_to_initial_pose_on_connect:=false

停止左臂节点后再执行：

ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=false \
  move_to_initial_pose_on_connect:=false

每侧检查：

1. 急停可用、工作区清空、速度保持低值。
2. 日志先出现首帧有效关节反馈，之后才允许按 Grip。
3. 静止握持时无启动跳变。
4. 只做小幅单轴位置和小角度姿态动作。
5. 松开 Grip、停止 XR 数据时均触发慢停止。
6. 人为制造不可达目标时不出现关节突跳；QP 失败保持上一组有效目标。
7. 左臂工具 TCP 使用 minisci +0.19 m，右臂使用 omnipic +0.16 m。
8. 停止一侧并确认连接释放后，才验证另一侧。

本轮不执行 arm:=both use_mock:=false。

• Step 8: 最终检查变更范围，不提交

git diff --stat
git diff -- \
  src/xr_rm_teleop \
  src/xr_rm_bringup \
  src/README.md

Expected: 只包含本计划列出的 QP、Adapter、launch、配置、模型、测试和文档变更。 在用户明确授权之前不创建提交。