From bfd50e10353f9adff5eda5924056efb09fcbbc60 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Mon, 27 Jul 2026 19:30:30 +0800 Subject: [PATCH] feat: add RM75 Placo single-step QP inverse kinematics design document --- .../plans/2026-07-27-rm75-placo-qp-ik.md | 1644 +++++++++++++++++ .../2026-07-27-rm75-placo-qp-ik-design.md | 284 +++ openspec/config.yaml | 20 - 3 files changed, 1928 insertions(+), 20 deletions(-) create mode 100644 docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md create mode 100644 docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md delete mode 100644 openspec/config.yaml diff --git a/docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md b/docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md new file mode 100644 index 0000000..ad1fbad --- /dev/null +++ b/docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md @@ -0,0 +1,1644 @@ +# 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、配置、模型、测试和文档变更。 +在用户明确授权之前不创建提交。 diff --git a/docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md b/docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md new file mode 100644 index 0000000..1d78901 --- /dev/null +++ b/docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md @@ -0,0 +1,284 @@ +# RM75 Placo 单步 QP 逆解设计 + +日期：2026-07-27 +状态：已批准，等待实施 + +## 1. 目标 + +将 `single_arm_velocity_teleop` 当前通过 `rm_movep_canfd` 调用睿尔曼控制器内部逆解的链路，替换为独立的 Placo QP 逆解： + +1. 每个 90 Hz 控制周期读取最新实际关节角。 +2. 将本周期工具 TCP 目标交给 Placo。 +3. 每周期只调用一次 `solver.solve(True)`。 +4. 得到 7 个目标关节角后，通过 `rm_movej_canfd(..., follow=False)` 控制 RM75。 + +实现借鉴 XRoboToolkit 真机示例的控制理念，但不依赖或复制 XRoboToolkit 项目代码。首轮分别独立验证左臂和右臂 RM75；实现本身继续支持现有左臂、右臂和双臂启动方式。 + +## 2. 不在本次范围内 + +- 不采用上传示例中的 Pinocchio + OSQP 多轮迭代逆解实现。 +- 不新增独立 ROS2 QP 求解节点。 +- 不改变 XR 输入协议、控制器话题或左右臂节点名。 +- 不关闭现有工作空间、圆柱、速度、超时或安全停止逻辑。 +- 不由 Codex 连接或移动真实机械臂、夹爪。 +- 不在本轮验证左右臂同时运行的双臂真机模式。 + +## 3. 选定方案 + +在 `xr_rm_teleop` 中新增轻量 `PlacoIkSolver`，由每个 `single_arm_velocity_teleop` 进程持有一个实例： + +- 遥操作节点继续负责 XR 相对位姿、滤波、死区、工作空间和速度限制。 +- `PlacoIkSolver` 负责 RM75 模型、工具 TCP/法兰变换以及单步 QP。 +- `RealManAdapter` 负责唯一的厂商 SDK 连接、关节反馈缓存、关节目标下发、安全停止和工具控制。 +- `MockRealManAdapter` 提供同一套关节反馈和关节目标接口，不导入厂商 SDK。 + +没有选择以下方案： + +- 将 Placo 逻辑继续堆入已有的大型遥操作节点：改动集中，但职责更混乱且难以独立测试。 +- 增加独立 QP ROS2 节点：隔离更强，但引入额外话题、时序和状态同步，对当前单机 90 Hz 控制没有必要。 + +## 4. 启动与连接生命周期 + +`launcher_ui.py` 不直接创建 Adapter。启动链路为： + +```text +launcher_ui.py + -> arm_debug.launch.py (arm:=left/right/both) + -> 对应 single_arm_velocity_teleop 节点 + -> 节点内部创建 PlacoIkSolver 和 Adapter +``` + +具体行为： + +- `arm:=left use_mock:=false`：一个左臂节点、一个求解器、一个左臂 RealMan 连接。 +- `arm:=right use_mock:=false`：一个右臂节点、一个求解器、一个右臂 RealMan 连接。 +- `arm:=both use_mock:=false`：左右节点各自持有一个求解器，并各自连接对应 IP。 +- `use_mock:=true`：创建 `MockRealManAdapter`，不加载厂商 SDK，不建立真机连接。 + +每个单臂节点只调用一次 `rm_create_robot_arm`。关节反馈、`rm_movej_canfd`、慢停止和工具控制复用同一个 SDK 句柄，不为反馈建立第二条连接，也不让一条连接控制两台机械臂。 + +## 5. RM75 模型与关节约束 + +将上传文件中的 `RM75-B.urdf` 及其网格作为 `xr_rm_teleop` 包资源安装，不携带上传示例的 Pinocchio、OSQP 或仿真控制代码。 + +模型约定： + +- 固定基座：`base_link`。 +- 运动关节：按 `joint_1` 到 `joint_7` 顺序映射 SDK 的 7 个关节角。 +- 末端法兰帧：`link_7`。 +- 节点和 Placo 内部统一使用弧度；Adapter 在 SDK 反馈/指令边界完成度与弧度转换。 + +Placo 启用 URDF 关节位置和速度限制。上传 URDF 中的位置范围与睿尔曼官方 RM75-B 范围一致： + +```text +J1 ±178°, J2 ±130°, J3 ±178°, J4 ±135°, +J5 ±178°, J6 ±128°, J7 ±360° +``` + +旧的、当前未被调用的 `fun_peripheral.alg_init()` 自定义限位不作为 QP 限位来源。控制器侧现有 `configure_safety_limits`、关节最大速度和最大加速度设置继续保留。 + +参考： + +- [睿尔曼 RM75-B 本体参数](https://develop.realman-robotics.com/robot/robotParameter/RM75OntologyParameters/) +- [XRoboToolkit DualArmURController](https://github.com/XR-Robotics/XRoboToolkit-Teleop-Sample-Python/blob/main/xrobotoolkit_teleop/hardware/dual_arm_ur_controller.py) + +## 6. 工具 TCP 处理 + +URDF 只描述到 `link_7`，实际工具来自 `peripherals_rm75.yaml`。同一份工具配置有两个使用者： + +```text +peripherals_rm75.yaml + ├─ RealManAdapter：设置真实控制器工具坐标系和负载 + └─ PlacoIkSolver：构造法兰到工具 TCP 的固定变换 +``` + +当前选择为： + +- 左臂 `scissorgripper: 2`：`minisci`，局部 Z 偏移 `+0.19 m`。 +- 右臂 `scissorgripper: 1`：`omnipic`，局部 Z 偏移 `+0.16 m`。 + +实现读取完整的 `[x, y, z, qx, qy, qz, qw]`，不硬编码为世界坐标 Z 偏移。设： + +- `B_T_F(q)`：Placo 由关节角计算的基座到法兰变换。 +- `F_T_T`：YAML 给出的法兰到工具 TCP 固定变换。 +- `B_T_T_target`：经过现有安全和速度限制后的目标工具 TCP。 + +正解和目标换算为： + +```text +B_T_T(q) = B_T_F(q) * F_T_T +B_T_F_target = B_T_T_target * inverse(F_T_T) +``` + +`F_T_T` 及其逆矩阵在启动时预计算。每周期只执行少量固定尺寸矩阵运算，不重新读取 YAML、求逆或加载 URDF。工作空间、圆柱限制、调试位姿和误差验收均以工具 TCP 为准；只有 Placo frame task 使用换算后的法兰目标。 + +## 7. Placo 求解器 + +每个求解器包含： + +- 一个 `placo.RobotWrapper`。Placo 0.9.4 会为模型加入 7 个虚拟浮动基座状态， + 因此 `robot.state.q` 长度为 14，真实 RM75 关节固定映射为 + `robot.state.q[7:14]`。 +- 一个 `placo.KinematicsSolver`，`dt = 1 / control_rate_hz`。 +- 一个作用于 `link_7` 的软约束完整位姿任务。 +- 一个可操作度任务。 +- 一个动能正则项。 +- 启用的关节位置与速度限制。 + +RM75 基座实际固定，创建求解器后必须调用 `solver.mask_fbase(True)`，禁止 QP +通过移动虚拟基座减小末端误差。所有状态同步和结果提取只读写 +`robot.state.q[7:14]`。 + +初始权重沿用 XR 真机示例的最小配置： + +```text +frame task: soft, 1.0 +manipulability task: soft, 5e-2 +kinetic energy regularizer: 1e-6 +``` + +每个周期先用实际关节反馈覆盖 Placo 状态并更新运动学，再设置法兰目标，最后只调用一次 `solver.solve(True)`。这里的“一步”指一次外层 Placo 求解调用；QP 求解器完成该次优化所需的内部数值迭代不算额外控制周期。 + +Placo 0.9.4 在 RM75 全零 neutral 位形下会出现 QP `NaN`；左右臂现有实际 +初始关节角的一步求解均能得到 7 个有限结果。因此全零位形不作为启动状态或 +健康检查，必须等待首帧实际关节反馈后才能启用 QP。 + +## 8. 90 Hz 数据流 + +```text +XR 相对位姿 + -> 现有死区、滤波、工作空间/圆柱限制 + -> 现有线速度和角速度单周期限制 + -> 目标工具 TCP + -> 换算目标法兰位姿 + -> 读取 Adapter 最新实际关节角 + -> 同步 Placo 状态 + -> solver.solve(True) 一次 + -> 校验 7 个目标关节角 + -> rad 转 deg + -> rm_movej_canfd(..., follow=False) +``` + +`RealManAdapter` 连接后在后台连续调用 `rm_get_joint_degree()`，把最新 7 关节角和单调时钟时间戳存入线程安全缓存。控制定时器只复制缓存，不在 90 Hz 回调中等待关节查询。缓存锁只保护内存数据，不包围网络调用。 + +第一次有效反馈到达前不调用 `solver.solve(True)`，也不发送运动命令。首帧必须 +包含 7 个有限关节角且未过期；收到后将度转换为弧度写入 +`robot.state.q[7:14]`，更新运动学，把当前工具 TCP 设为初始目标，并以实际 +关节角初始化 `last_valid_joint_target`。Mock 模式使用现有 +`initial_joint_pose` 初始化 7 关节状态并立即提供同样的首帧有效反馈，再通过 +同一 Placo 正解计算工具 TCP；原先仅用于笛卡尔 mock 的 +`mock_initial_pose` 随旧控制链路移除。 + +原 `rm_movep_canfd` 不再位于遥操作运动链路中。 + +## 9. 异常与停止策略 + +启动时先校验 Placo、URDF、关节顺序和工具配置，成功后才连接真机。运行时分为两类异常。 + +### 9.1 沿用 XR 的 last-known-good 策略 + +第一帧有效关节反馈到达后，用实际关节角初始化 `last_valid_joint_target`。 + +- QP 成功且输出通过校验：更新并发送新的 `last_valid_joint_target`。 +- QP 抛出异常、返回错误维数、`NaN/Inf`，或输出违反关节位置/单周期速度限制：不更新目标，继续发送上一组有效关节目标。 +- 下一周期 QP 恢复：自动恢复目标更新，不要求重新按 Grip。 +- 求解失败日志限频，避免日志影响控制周期。 + +不可达目标本身不视为求解异常；软约束任务继续在约束内每周期靠近一步。 + +### 9.2 输入、反馈或通信不可信时慢停止 + +以下情况不使用旧关节目标，沿用现有只发送一次慢停止并重置激活状态的逻辑： + +- XR 指令超过现有 `command_timeout_sec`。 +- Grip 松开。 +- 真实关节反馈没有首帧、过期、维数错误或包含 `NaN/Inf`。 +- SDK 关节指令发送失败。 +- 四元数非法。 +- 节点关闭。 + +关节反馈时效先复用现有 `command_timeout_sec=0.12`，避免增加含义相近的参数。若真机测量证明正常反馈无法稳定满足该阈值，再单独拆分反馈超时参数。 + +`configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 的启动默认值保持 `false`。 + +## 10. 依赖、Python 环境与配置 + +- 复用现有 `/home/robot/miniconda3/envs/xr` 环境及其中已经验证的 + Placo `0.9.4`、Pin `3.7.0` 和 NumPy `2.2.6`，不新增 XRoboToolkit + 项目依赖。 +- `arm_debug.launch.py` 明确使用 + `/home/robot/miniconda3/envs/xr/bin/python` 启动 + `single_arm_velocity_teleop`；ROS2 launch 和 `colcon` 仍使用系统 + `/usr/bin/python3`。 +- 禁止升级 Placo，禁止向系统 Python、`pip --user` 或其他全局位置安装 + Placo、Pinocchio、EigenPy 或 NumPy。构建不改用 Conda Python。 +- launch 启动前校验 XR Python 路径存在；不存在时直接报错，不回退到可能 + 缺少 Placo 或版本不同的系统 Python。 +- 真机模式继续按需导入睿尔曼 Python API2。 +- Mock 模式依赖 Placo 和 RM75 模型，但不得导入或要求安装睿尔曼 SDK。 +- 工具选择继续只由 `peripherals_rm75.yaml` 和现有 `peripheral_arm` 决定。 +- 左、右、双臂 YAML 中与 QP 相关的共同配置保持一致；左右现有空间、映射和初始关节角保持各自配置。 +- 将左右单臂 YAML 的 `move_to_initial_pose_on_connect` 默认值统一为 `false`，并同步 README；需要自动回初始位姿时必须由用户显式传 `true`。 +- 不新增“为以后准备”的插件接口、求解器工厂或额外 ROS 消息。 + +## 11. 验证与验收 + +### 11.1 自动验证 + +- 工具 TCP/法兰变换可往返，包含末端旋转后的局部 Z 偏移。 +- RM75 URDF 能加载，且映射顺序严格为 `joint_1` 到 `joint_7`。 +- 使用 Placo 0.9.4 时固定虚拟基座，真实关节只映射 + `robot.state.q[7:14]`。 +- 没有首帧有效关节反馈时不调用 QP、不发送关节目标；首帧到达后用实际关节角 + 初始化状态和 `last_valid_joint_target`。 +- 一次 QP 求解输出 7 个有限关节角并满足位置、单周期速度限制。 +- 强制 QP 失败时继续使用上一组有效关节目标。 +- 强制反馈过期时执行慢停止。 +- Mock 模式不导入睿尔曼 SDK。 +- 运行现有姿态控制测试： + + ```bash + pytest src/xr_rm_teleop/test/test_orientation_control.py + ``` + +- 从工作空间根目录构建： + + ```bash + source /opt/ros/humble/setup.bash + colcon build --symlink-install + ``` + +### 11.2 左右臂单独 Mock 验收 + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true +``` + +左臂和右臂必须分别独立启动并完成相同验收。每个单臂目标停止变化并保持 `0.5 s` 后： + +- 工具 TCP 位置误差不超过 `5 mm`。 +- 工具 TCP 姿态误差不超过 `2°`。 +- 记录 Placo 单次求解耗时和控制周期超时情况。 +- 左臂使用 `minisci +0.19 m` 工具变换，右臂使用 `omnipic +0.16 m` 工具变换。 + +90 Hz 的周期预算约为 `11.1 ms`。性能数据作为验证报告输出，不把易受机器负载影响的耗时阈值写成单元测试硬断言。 + +### 11.3 左右臂单独真机验收 + +Codex 分别提供 `launcher_ui.py` 左臂、右臂启动步骤和检查清单，不执行真机连接、运动或夹爪操作。用户在确认急停、障碍物、低速和初始姿态后，先只启动一侧完成验证，停止该侧节点后再验证另一侧。本轮不以 `arm:=both` 进行真机验收。两侧真机首次启动都必须保持 `move_to_initial_pose_on_connect:=false`。 + +## 12. 完成标准 + +满足以下条件才视为实现完成： + +1. 遥操作运动链路不再调用 `rm_movep_canfd`。 +2. 每个有效控制周期只有一次 Placo `solve(True)`。 +3. 目标通过 7 个关节角和 `rm_movej_canfd` 下发。 +4. 同一机械臂始终只有一个 RealMan SDK 连接。 +5. 工具 TCP 偏移参与目标换算、正解和误差验收。 +6. QP 失败使用上一组有效目标，输入/反馈/通信失败执行慢停止。 +7. 指定构建、测试以及左臂、右臂各自的 mock 验收通过。 +8. 分别提供左臂、右臂真机人工验证步骤，但不代替用户执行。 +9. 遥操作节点由 launch 显式使用 XR Python 和 Placo 0.9.4，未升级或全局安装 + 数值依赖。 diff --git a/openspec/config.yaml b/openspec/config.yaml deleted file mode 100644 index 392946c..0000000 --- a/openspec/config.yaml +++ /dev/null @@ -1,20 +0,0 @@ -schema: spec-driven - -# Project context (optional) -# This is shown to AI when creating artifacts. -# Add your tech stack, conventions, style guides, domain knowledge, etc. -# Example: -# context: | -# Tech stack: TypeScript, React, Node.js -# We use conventional commits -# Domain: e-commerce platform - -# Per-artifact rules (optional) -# Add custom rules for specific artifacts. -# Example: -# rules: -# proposal: -# - Keep proposals under 500 words -# - Always include a "Non-goals" section -# tasks: -# - Break tasks into chunks of max 2 hours