# 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: 从用户上传包复制且只复制模型资源** 从工作空间根目录执行： ```bash 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 代码： ```bash 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`： ```python 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: 运行测试并确认它因模块尚不存在而失败** ```bash 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` 中加入： ```python @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()` 中原来的工具名选择替换为： ```python tool_name = config.tool_name ``` 工具位姿和控制器工具帧由同一个 `PeripheralConfig` 提供，避免左右臂工具索引在两个位置各自解析。 - [ ] **Step 5: 实现不在系统 Python 导入 Placo 的变换和求解器骨架** 创建 `src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`，模块顶层只导入标准库和 NumPy；`placo` 必须在构造函数内导入： ```python """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: 运行纯变换测试** ```bash 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 变更范围** ```bash 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`： ```python 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: 运行脚本并确认求解器类尚不存在** ```bash 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` 追加： ```python 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`，并追加： ```python 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)) ``` 运行： ```bash 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 验收** ```bash 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: ```text 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 变更，不提交** ```bash 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` 测试，并追加： ```python 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 不满足测试** ```bash 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`： ```python import threading import time ``` 加入： ```python @dataclass(frozen=True) class JointStateSnapshot: positions: list[float] received_at: float ``` 将 `MockRealManAdapter` 替换成： ```python 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` 替换为： ```python feedback_period: float, ``` 在构造函数末尾加入： ```python 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`，在安全配置和可选初始移动之后启动反馈线程： ```python 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() ``` 加入以下方法： ```python 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 的签名统一为： ```python def configure_peripheral(self, config, peripheral_arm: str) -> None: ``` 真机实现不再读取 YAML，直接使用： ```python 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()` 改为： ```python 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 与姿态回归测试** ```bash 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: 静态确认只有一个连接创建点** ```bash 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`： ```python 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: 运行测试并确认节点尚无这些方法** ```bash 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 中加入： ```python import time from .fun_peripheral import load_peripheral_config from .placo_ik_solver import PlacoIkSolver from .realman_adapter import ( ArmPose, JointStateSnapshot, MockRealManAdapter, RealManAdapter, ) ``` 声明新参数： ```python self.declare_parameter("robot_urdf_path", "") ``` 删除已失效的参数声明和成员读取： ```text current_pose_poll_hz mock_initial_pose frame_type ``` 初始化字段加入： ```python self._last_valid_joint_target: list[float] | None = None self._joint_feedback_ready = False ``` 把 Adapter 创建顺序替换为： ```python 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()` 改为： ```python 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()` 开头先执行控制器工具配置： ```python peripheral_arm = self._peripheral_arm_name() if self._bool_parameter("configure_peripheral_on_connect"): self._adapter.configure_peripheral( self._peripheral_config, peripheral_arm, ) ``` 然后再判断： ```python if not self._enable_tool_control: ``` 删除该函数中重复读取 `peripheral_config_file` 和重复调用旧 `configure_peripheral(config_file, peripheral_arm)` 的代码。 - [ ] **Step 6: 实现反馈时效、首帧初始化和 QP 失败回退** 在节点类中加入： ```python 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 消息前加入： ```python 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： ```python self._enter_active_control( controller_now, controller_quat, current_pose, now, ) ``` 函数签名改为： ```python 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` 的全部死区、滤波、工作空间/圆柱、线速度和角速度限制。 把： ```python if self._send_cartesian_target(target_pose): ``` 替换为： ```python joint_target = self._solve_joint_target(target_pose) if self._send_joint_target(joint_target): ``` 删除以下旧方法： ```text _read_current_pose_for_control _maybe_refresh_current_pose _send_cartesian_target ``` 每个有效且激活的控制周期只能有一次 `self._ik_solver.solve(target_pose)`。 - [ ] **Step 9: 运行节点行为和姿态回归测试** ```bash 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: 静态确认旧笛卡尔运动链路已移除** ```bash 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 依赖** 加入： ```python from glob import glob ``` 把 `data_files` 扩展为： ```python 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"), ), ], ``` 保持： ```python install_requires=["setuptools"], ``` 不得加入 `placo`、`pin`、`eigenpy` 或 `numpy`，以免 colcon 使用系统 Python 时触发安装或升级。 - [ ] **Step 2: launch 固定并验证 XR Python** 在 `arm_debug.launch.py` 加入： ```python from pathlib import Path ``` 定义： ```python XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python" ``` 在 `_launch_setup()` 最前面校验： ```python 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 对象都加入： ```python prefix=[XR_PYTHON], ``` 不要给 `udp_controller_receiver` 加 prefix；它继续使用系统 ROS2 Python。 - [ ] **Step 3: launch 向每个遥操作节点传安装后的 URDF** 给三个遥操作 `Node` 的参数字典都加入： ```python "robot_urdf_path": PathJoinSubstitution([ FindPackageShare("xr_rm_teleop"), "models", "rm75", "RM75-B.urdf", ]), ``` 删除 launch 中已经失效的 `frame_type` 参数传递、函数参数和对应的 `DeclareLaunchArgument`。 将控制周期注释更新为： ```python # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。 ``` - [ ] **Step 4: 同步左、右、双臂 YAML** 三个 YAML 都删除： ```text current_pose_poll_hz mock_initial_pose frame_type ``` 保留每侧原有： ```text initial_joint_pose workspace_min / workspace_max cyl_radius_limit max_linear_speed / max_orientation_speed configure_safety_limits: true ``` 把左右单臂 YAML 都改为： ```yaml move_to_initial_pose_on_connect: false ``` 双臂文件原有左右两个 `false` 保持不变。把 `rm_movep_canfd` 注释改为 “Placo 单步 QP + `rm_movej_canfd` 关节透传”。 - [ ] **Step 5: 从工作空间根目录构建** ```bash cd /home/robot/WS_xr source /opt/ros/humble/setup.bash colcon build --symlink-install ``` Expected: build summary 无失败包。 - [ ] **Step 6: 检查模型和 launch 安装结果** ```bash 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: 使用安装模型再次运行左右臂数值验收** ```bash 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 控制链路** 把项目首页链路改为： ```text 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 关节目标 ``` 明确说明： ```text 遥操作节点收到首帧有效实际关节反馈前不调用 QP、不发送运动指令。 QP 失败时沿用上一组有效关节目标；XR 超时、反馈过期、Grip 松开或 SDK 发送失败时执行慢停止。 ``` - [ ] **Step 2: 更新环境说明** 在“环境准备”加入： ```text 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。 ``` 加入只读检查命令： ```bash /home/robot/miniconda3/envs/xr/bin/python -c \ "import importlib.metadata; print(importlib.metadata.version('placo'))" ``` Expected: `0.9.4`。 - [ ] **Step 3: 更新参数和真机安全说明** 删除 README 对以下旧参数的描述： ```text frame_type current_pose_poll_hz mock_initial_pose ``` 把 `follow` 描述改成传给 `rm_movej_canfd`。把单臂自动回初始位姿说明改成： ```text 左臂、右臂和双臂默认都不会自动执行 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: 运行完整自动测试** ```bash 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** 左臂： ```bash 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 ``` 右臂： ```bash 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: 最终静态安全检查** ```bash 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 执行： ```bash ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=left use_mock:=false \ move_to_initial_pose_on_connect:=false ``` 停止左臂节点后再执行： ```bash 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: 最终检查变更范围，不提交** ```bash git diff --stat git diff -- \ src/xr_rm_teleop \ src/xr_rm_bringup \ src/README.md ``` Expected: 只包含本计划列出的 QP、Adapter、launch、配置、模型、测试和文档变更。 在用户明确授权之前不创建提交。