diff --git a/AGENTS.md b/AGENTS.md
index e52a7e6..79e725b 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -227,7 +227,9 @@
## Git 与提交
-除非用户明确要求，否则不要自动提交、推送、创建分支或修改远程仓库。
+除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
+
+使用 Superpowers 执行计划时，允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支；其他情况下，除非用户明确要求，不要自动创建分支。
如果用户要求生成提交信息，提交信息应：
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
deleted file mode 100644
index ad1fbad..0000000
--- a/docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md
+++ /dev/null
@@ -1,1644 +0,0 @@
-# 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/plans/2026-07-28-rm75-control-timing-stats.md b/docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md
deleted file mode 100644
index 8551124..0000000
--- a/docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md
+++ /dev/null
@@ -1,154 +0,0 @@
-# RM75 Control Timing Stats 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:** 在 Grip 激活期间每约 5 秒向 `arm_debug.launch.py` 终端输出一次控制链路耗时统计。
-
-**Architecture:** 在现有 `SingleArmVelocityTeleop` 控制回调内使用单调高精度时钟记录实际周期、控制路径总耗时、QP、关节发送和反馈年龄。节点保存一个固定长度样本窗口，满窗后用 NumPy 计算 mean/P95/P99/max，输出一条 ROS 日志并清空窗口。
-
-**Tech Stack:** Python 3.10、ROS2 Humble `rclpy`、NumPy、pytest。
-
----
-
-### Task 1: 控制周期统计
-
-**Files:**
-- Modify: `xr_rm_teleop/test/test_joint_control.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-
-- [x] **Step 1: 写失败测试**
-
-在 `test_joint_control.py` 添加确定性两样本窗口测试：
-
-```python
-def test_timing_stats_logs_summary_and_clears_window() -> None:
- messages = []
- teleop = object.__new__(SingleArmVelocityTeleop)
- teleop._arm_name = "right_rm75"
- teleop._dt = 0.008
- teleop._timing_stats_window = 2
- teleop._timing_samples = {
- name: []
- for name in ("period", "total", "qp", "send", "feedback_age")
- }
- teleop.get_logger = lambda: SimpleNamespace(
- info=lambda message: messages.append(message)
- )
-
- teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
- assert messages == []
-
- teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
-
- assert len(messages) == 1
- assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
- assert "period[n=2 mean=8.000 p95=8.900 p99=8.980 max=9.000 ms overruns=1]" in messages[0]
- assert "total[n=2 mean=8.000 p95=9.800 p99=9.960 max=10.000 ms overruns=1]" in messages[0]
- assert "qp[n=2" in messages[0]
- assert "send[n=2" in messages[0]
- assert "feedback_age[n=2" in messages[0]
- assert all(not samples for samples in teleop._timing_samples.values())
-```
-
-- [x] **Step 2: 确认测试因功能缺失而失败**
-
-在工作空间根目录运行：
-
-```bash
-source /opt/ros/humble/setup.bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
-```
-
-预期：失败并提示 `SingleArmVelocityTeleop` 没有 `_record_timing_sample`。
-
-- [x] **Step 3: 实现最小统计逻辑**
-
-在节点初始化中创建约 5 秒的窗口：
-
-```python
-self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
-self._timing_samples = {
- name: []
- for name in ("period", "total", "qp", "send", "feedback_age")
-}
-self._last_control_tick_started_ns: int | None = None
-```
-
-为每组样本计算统计摘要：
-
-```python
-def _timing_summary(
- self,
- name: str,
- samples: list[float],
- deadline_ms: float | None = None,
-) -> str:
- values = np.asarray(samples)
- result = (
- f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
- f"p95={np.percentile(values, 95):.3f} "
- f"p99={np.percentile(values, 99):.3f} "
- f"max={np.max(values):.3f} ms"
- )
- if deadline_ms is not None:
- result += f" overruns={np.count_nonzero(values > deadline_ms)}"
- return result + "]"
-```
-
-满窗后输出并清空：
-
-```python
-def _record_timing_sample(
- self,
- period_ms: float | None,
- total_ms: float,
- qp_ms: float,
- send_ms: float,
- feedback_age_ms: float,
-) -> None:
- if period_ms is not None:
- self._timing_samples["period"].append(period_ms)
- self._timing_samples["total"].append(total_ms)
- self._timing_samples["qp"].append(qp_ms)
- self._timing_samples["send"].append(send_ms)
- self._timing_samples["feedback_age"].append(feedback_age_ms)
- if len(self._timing_samples["total"]) < self._timing_stats_window:
- return
-
- deadline_ms = self._dt * 1000.0
- summaries = [
- self._timing_summary("period", self._timing_samples["period"], deadline_ms),
- self._timing_summary("total", self._timing_samples["total"], deadline_ms),
- self._timing_summary("qp", self._timing_samples["qp"]),
- self._timing_summary("send", self._timing_samples["send"]),
- self._timing_summary("feedback_age", self._timing_samples["feedback_age"]),
- ]
- self.get_logger().info(
- f"{self._arm_name} timing n={len(self._timing_samples['total'])} "
- f"deadline={deadline_ms:.3f} ms | " + " | ".join(summaries)
- )
- for samples in self._timing_samples.values():
- samples.clear()
-```
-
-在 `_control_tick()` 中围绕 QP 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。
-
-- [x] **Step 4: 运行测试确认通过**
-
-```bash
-source /opt/ros/humble/setup.bash
-PYTHONPATH=src/xr_rm_teleop pytest -q src/xr_rm_teleop/test/test_joint_control.py
-```
-
-预期：全部通过。
-
-- [x] **Step 5: 完整验证**
-
-```bash
-source /opt/ros/humble/setup.bash
-pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-colcon build --symlink-install
-```
-
-预期：姿态测试和工作空间构建全部通过。根据仓库规则，不自动提交 Git。
diff --git a/docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md b/docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md
deleted file mode 100644
index bccfdef..0000000
--- a/docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md
+++ /dev/null
@@ -1,109 +0,0 @@
-# RM75 Feedback Absolute Scheduling 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:** 将关节反馈线程从“读取后固定等待 8 ms”改为无历史周期补跑的绝对起始周期调度。
-
-**Architecture:** `RealManAdapter._feedback_loop()` 保留现有读取、告警和停止结构，只把固定 `Event.wait(feedback_period)` 替换为下一截止时间计算。读取提前完成时等待剩余时间；读取超期时重置调度基准并立即进入下一周期。
-
-**Tech Stack:** Python 3.10、threading、time.monotonic、pytest、ROS2 Humble、colcon
-
----
-
-### Task 1: 反馈绝对周期调度
-
-**Files:**
-- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
-- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
-
-- [ ] **Step 1: 写失败测试**
-
-用确定性的 FakeTime 和 FakeStopEvent 运行 `_feedback_loop()` 三次读取：
-
-- 第一次读取在 5 ms 完成，应只等待剩余 3 ms；
-- 第二次在 18 ms 完成，超过 16 ms 截止时间，应不等待并把基准重置为
- 18 ms；
-- 第三次在 23 ms 完成，应等待到新基准的 26 ms，即再次等待 3 ms。
-
-断言读取三次且 `wait()` 参数为 `[0.003, 0.003]`。该结果同时证明没有补跑
-旧的 8 ms 和 16 ms 截止点。
-
-- [ ] **Step 2: 确认测试失败**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-预期：当前固定等待实现记录 `[0.008, 0.008, 0.008]`，测试失败。
-
-- [ ] **Step 3: 写最小实现**
-
-把 `_feedback_loop()` 改为：
-
-```python
-def _feedback_loop(self) -> None:
- next_read_at = time.monotonic()
- 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
-
- next_read_at += self._feedback_period
- remaining = next_read_at - time.monotonic()
- if remaining <= 0.0:
- next_read_at = time.monotonic()
- continue
- self._feedback_stop.wait(remaining)
-```
-
-- [ ] **Step 4: 确认局部测试通过**
-
-重复 Step 2 命令。预期：全部 PASS。
-
-### Task 2: 调度回归验证
-
-**Files:**
-- Verify: `xr_rm_teleop`
-- Verify: ROS2 workspace
-
-- [ ] **Step 1: 运行遥操作包测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-```
-
-预期：全部 PASS。
-
-- [ ] **Step 2: 运行姿态控制指定测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-```
-
-预期：全部 PASS。
-
-- [ ] **Step 3: 构建工作空间**
-
-```bash
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-```
-
-预期：四个包构建成功。
-
-- [ ] **Step 4: 检查最终差异**
-
-```bash
-git diff --check
-git status --short
-```
-
-预期：只包含已确认的统计、工具坐标系幂等修复、反馈绝对周期调度、对应测试
-及 Superpowers 文档。按仓库规则不自动提交。
diff --git a/docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md b/docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md
deleted file mode 100644
index 5b3d2c7..0000000
--- a/docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md
+++ /dev/null
@@ -1,167 +0,0 @@
-# RM75 Feedback Thread Timing 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:** 在不改变反馈轮询和机械臂控制行为的前提下，统计 `rm_get_joint_degree()` 调用耗时与成功反馈更新间隔。
-
-**Architecture:** `RealManAdapter` 在反馈读取边界测量时间，并把可选计时值随 `JointStateSnapshot` 放入现有缓存。`SingleArmVelocityTeleop` 复用现有 timing 窗口，只对新的反馈时间戳记录一次并输出汇总。
-
-**Tech Stack:** Python 3.10、ROS2 Humble、pytest、NumPy、colcon
-
----
-
-### Task 1: 在反馈缓存中携带真实读取计时
-
-**Files:**
-- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
-- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
-
-- [ ] **Step 1: 写失败测试**
-
-在 `test_joint_feedback_is_cached_in_radians` 中通过 `monkeypatch` 固定
-`perf_counter_ns()` 和 `monotonic()`，连续读取两次，并验证：
-
-```python
-assert first.read_duration_ms == pytest.approx(2.0)
-assert first.update_interval_ms is None
-assert second.read_duration_ms == pytest.approx(3.0)
-assert second.update_interval_ms == pytest.approx(11.0)
-```
-
-- [ ] **Step 2: 确认测试失败**
-
-在 `/home/robot/WS_xr` 执行：
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py::test_joint_feedback_is_cached_in_radians
-```
-
-预期：因 `JointStateSnapshot` 尚无计时字段而失败。
-
-- [ ] **Step 3: 最小实现反馈计时**
-
-给快照增加可选字段，保持现有两参数构造兼容：
-
-```python
-@dataclass(frozen=True)
-class JointStateSnapshot:
- positions: list[float]
- received_at: float
- read_duration_ms: float | None = None
- update_interval_ms: float | None = None
-```
-
-在 `_read_joint_state_once()` 中只包围 SDK 调用测量 `read_duration_ms`；数据校验
-成功后取得 `received_at`，并在缓存锁内根据上一快照计算
-`update_interval_ms`。`get_latest_joint_state()` 同步复制两个字段。Mock 使用
-字段默认值，不伪造计时。
-
-- [ ] **Step 4: 确认局部测试通过**
-
-重复 Step 2 命令。预期：PASS。
-
-### Task 2: 将唯一反馈样本加入现有 timing 汇总
-
-**Files:**
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-- Test: `xr_rm_teleop/test/test_joint_control.py`
-
-- [ ] **Step 1: 写失败测试**
-
-扩展 `test_timing_stats_logs_summary_and_clears_window`：在三个控制样本中传入
-“快照 A、重复快照 A、快照 B”，并断言日志包含：
-
-```python
-assert "feedback_read[n=2" in messages[0]
-assert "feedback_interval[n=1" in messages[0]
-```
-
-这样同时验证新反馈只计一次、重复缓存不重复计数、首次反馈无更新间隔。
-
-- [ ] **Step 2: 确认测试失败**
-
-在 `/home/robot/WS_xr` 执行：
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
-```
-
-预期：因 timing 样本尚不支持新字段而失败。
-
-- [ ] **Step 3: 最小实现唯一反馈统计**
-
-在节点初始化时：
-
-```python
-self._timing_samples = {
- name: []
- for name in (
- "period",
- "total",
- "qp",
- "send",
- "feedback_age",
- "feedback_read",
- "feedback_interval",
- )
-}
-self._last_timing_feedback_received_at: float | None = None
-```
-
-让 `_record_timing_sample()` 接收当前 `JointStateSnapshot`。仅当
-`received_at` 与 `_last_timing_feedback_received_at` 不同时，追加非 `None`
-的读取耗时和更新间隔。汇总时仅输出非空的新数组，避免 mock 模式对空数组
-求百分位数。控制循环把已有 `snapshot` 传入该函数。
-
-- [ ] **Step 4: 确认相关测试通过**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-预期：全部 PASS。
-
-### Task 3: 回归验证
-
-**Files:**
-- Verify: `xr_rm_teleop`
-- Verify: ROS2 workspace
-
-- [ ] **Step 1: 运行遥操作包测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-```
-
-预期：全部 PASS。
-
-- [ ] **Step 2: 运行姿态控制指定测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-```
-
-预期：全部 PASS。
-
-- [ ] **Step 3: 构建工作空间**
-
-```bash
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-```
-
-预期：四个包构建成功。
-
-- [ ] **Step 4: 检查差异**
-
-```bash
-git diff --check
-git status --short
-```
-
-预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。
diff --git a/docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md b/docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md
deleted file mode 100644
index d9c061b..0000000
--- a/docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md
+++ /dev/null
@@ -1,107 +0,0 @@
-# RM75 Idempotent Tool Frame 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:** 让 RealMan 工具坐标系在首次启动时创建、后续启动时更新，并检查所有相关 SDK 返回值。
-
-**Architecture:** `fun_peripheral.py` 增加一个只负责工具坐标系的内部函数，先查询名称列表，再选择创建或更新，最后切换。现有 `peripheral_cfg()` 继续负责 IO 和夹爪初始化，只把原来的两次无检查调用替换为该函数。
-
-**Tech Stack:** Python 3.10、RealMan Python API2、pytest、ROS2 Humble、colcon
-
----
-
-### Task 1: 工具坐标系幂等配置
-
-**Files:**
-- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
-- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
-
-- [ ] **Step 1: 写失败测试**
-
-导入新的 `_configure_tool_frame`，用 FakeArm 分别返回包含和不包含 `omnipic`
-的名称列表。断言不存在时调用 `rm_set_manual_tool_frame`，存在时调用
-`rm_update_tool_frame`，两条路径最后都调用 `rm_change_tool_frame`。
-
-再用参数化失败返回码验证查询、创建/更新和切换失败均抛出包含 SDK 操作名称
-的 `RuntimeError`。
-
-- [ ] **Step 2: 确认测试失败**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-预期：因 `_configure_tool_frame` 尚不存在而在测试收集阶段失败。
-
-- [ ] **Step 3: 写最小实现**
-
-在 `fun_peripheral.py` 增加：
-
-```python
-def _check_sdk_return(result: Any, operation: str) -> None:
- if result != 0:
- raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
-
-
-def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
- frames = robot.rm_get_total_tool_frame()
- if not isinstance(frames, dict):
- raise RuntimeError(
- f"rm_get_total_tool_frame returned invalid data: {frames!r}"
- )
- _check_sdk_return(
- frames.get("return_code"),
- "rm_get_total_tool_frame",
- )
- tool_names = frames.get("tool_names")
- if not isinstance(tool_names, (list, tuple)):
- raise RuntimeError(
- f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
- )
-
- if tool_name in tool_names:
- operation = "rm_update_tool_frame"
- result = robot.rm_update_tool_frame(frame=tool_frame)
- else:
- operation = "rm_set_manual_tool_frame"
- result = robot.rm_set_manual_tool_frame(frame=tool_frame)
- _check_sdk_return(result, operation)
- _check_sdk_return(
- robot.rm_change_tool_frame(tool_name),
- "rm_change_tool_frame",
- )
-```
-
-在 `peripheral_cfg()` 中用
-`_configure_tool_frame(robot, tool_frame, tool_name)` 替换原来的创建和切换
-调用。
-
-- [ ] **Step 4: 确认测试通过**
-
-重复 Step 2 命令。预期：全部 PASS。
-
-### Task 2: 工具修复回归验证
-
-**Files:**
-- Verify: `xr_rm_teleop`
-- Verify: ROS2 workspace
-
-- [ ] **Step 1: 运行遥操作包测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-```
-
-预期：全部 PASS。
-
-- [ ] **Step 2: 构建工作空间**
-
-```bash
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-```
-
-预期：四个包构建成功。
-
diff --git a/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md b/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md
deleted file mode 100644
index f27af99..0000000
--- a/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md
+++ /dev/null
@@ -1,390 +0,0 @@
-# RM75 SO(3) 姿态跟随与 OmniPicker 模型 Implementation Plan
-
-> **For Codex:** REQUIRED SUB-SKILL: Use `superpowers:executing-plans` to implement this plan task-by-task.
-
-**Goal:** 去掉遥操作控制路径中的 RPY 往返转换，使 RM75 TCP 姿态始终沿 SO(3) 最短路径跟随，并让左右臂的 Placo QP 直接控制一体化模型中的 `omnipicker_tcp`。
-
-**Architecture:** 保留现有单节点、单步 Placo QP、关节反馈、RealMan 连接和安全停止链路。XR 四元数映射为机器人旋转矩阵；平移使用直接位置差，姿态使用 SO(3) 对数误差，二者以解耦 `3+3` 形式处理。Placo 接收完整 `4×4` 目标矩阵并直接约束 URDF 的 `omnipicker_tcp`，不再读取外设工具位姿做 QP 末端换算。
-
-**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、NumPy、Placo 0.9.4、Pinocchio 3.7.0、pytest、URDF。
-
-**Repository rule:** 不执行 `git commit`、`git push` 或真机命令。所有启动验证必须显式使用 `use_mock:=true`；`peripherals_rm75.yaml`、`avoid_singularity`、可操作度任务和既有安全限制保持不变。
-
----
-
-## 文件范围
-
-- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
-- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
-- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
-- Modify: `xr_rm_teleop/setup.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-- Modify: `xr_rm_teleop/test/test_orientation_control.py`
-- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
-- Modify: `xr_rm_teleop/test/test_joint_control.py`
-- Modify: `xr_rm_teleop/test/placo_ik_smoke.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`
-- Modify: `README.md`
-
-不删除旧 `xr_rm_teleop/models/rm75` 资源，只让 launch 停止选用它，避免扩大无关清理范围。
-
-### Task 1: 导入 fixed 一体化模型并定义 TCP
-
-**Files:**
-
-- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
-- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
-- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
-- Modify: `xr_rm_teleop/setup.py`
-- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
-
-- [x] **Step 1: 先写模型结构失败测试**
-
-在 `test_placo_transforms.py` 中用 `xml.etree.ElementTree` 读取 fixed URDF，断言：
-
-```python
-assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
-assert tcp_joint.attrib["type"] == "fixed"
-assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
-assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
-assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
-assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
-```
-
-运行：
-
-```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，模型包尚不存在。
-
-- [x] **Step 2: 从上传 ZIP 只导入运行所需资源**
-
-从
-`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
-导入 fixed URDF 和两组 mesh 到
-`xr_rm_teleop/models/rm75_omnipicker`；不导入独立描述包元数据、示例脚本、
-活动式 URDF 或额外验证文档。保留上传模型的几何、惯量、关节限制和 fixed
-OmniPicker 关节，并在 `xr_rm_teleop/setup.py` 中安装这些资源。
-
-- [x] **Step 3: 在 fixed URDF 增加已确认的 TCP**
-
-```xml
-
-
-
-
-
-
-```
-
-- [x] **Step 4: 重跑模型测试**
-
-Expected: PASS；运动关节仍严格为 `joint_1` 至 `joint_7`，TCP 偏移为
-`+Z 0.16 m`。
-
-### Task 2: 让 Placo 直接接收 SE(3) 并约束 `omnipicker_tcp`
-
-**Files:**
-
-- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
-- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
-- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
-
-- [x] **Step 1: 先把变换和 smoke 测试改为矩阵接口**
-
-测试改为：
-
-```python
-current = solver.update_joint_state(joints)
-assert current.shape == (4, 4)
-target = current.copy()
-target[0, 3] += 0.01
-target[:3, :3] = rotation_delta @ target[:3, :3]
-joints = solver.solve(target)
-```
-
-同时覆盖非法形状、NaN 和非 SE(3) 最后一行会被拒绝。smoke 使用
-`dt=1/125`，以旋转矩阵相对角度计算姿态误差，不再转换 RPY。
-
-运行现有两项测试，确认它们先因旧 `ArmPose/tool_pose` 接口失败。
-
-- [x] **Step 2: 最小化求解器接口**
-
-将构造函数改为：
-
-```python
-PlacoIkSolver(urdf_path: str, dt: float)
-```
-
-并完成以下替换：
-
-- 删除 `_rpy_to_rotation`、`_rotation_to_rpy`、`_arm_pose_to_transform`、
- `_transform_to_arm_pose`、`_tool_pose_to_transform`。
-- 删除 `_tool_transform` 和 `_tool_inverse`。
-- frame task 从 `link_7` 改为 `omnipicker_tcp`。
-- 可操作度任务继续作用于原来的 `link_7`，并保留原权重
- `soft, 5e-2`。
-- `update_joint_state()` 直接返回
- `get_T_world_frame("omnipicker_tcp").copy()`。
-- `solve()` 校验并直接设置传入的 `4×4` 目标矩阵。
-- frame task、动能正则、虚拟基座固定、关节位置/速度校验保持原状。
-
-- [x] **Step 3: 运行纯单元测试**
-
-```bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_placo_transforms.py
-```
-
-Expected: PASS。该命令不构造 Placo，不要求系统 Python 安装厂商 SDK。
-
-### Task 3: 用 SO(3) 最短路径替换 RPY 姿态控制
-
-**Files:**
-
-- Modify: `xr_rm_teleop/test/test_orientation_control.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-
-- [x] **Step 1: 先写 SO(3) 回归测试**
-
-保留零四元数停止测试，并增加以下最小覆盖：
-
-- `q` 与 `-q` 得到同一旋转矩阵。
-- 初始 pitch 接近 `+90°`、`-90°` 时，小手柄旋转只产生同量级的小旋转。
-- 跨过旧 RPY 分支时，相对旋转仍取最短路径。
-- 死区按 `norm(Log(R_target R_currentᵀ))` 判断。
-- `alpha=0.5` 时 SO(3) 误差角减半。
-- `dt=1/125`、`max_orientation_speed=0.5` 时单步不超过 `0.004 rad`。
-- 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
-- 矩阵转调试四元数后有限且单位化。
-
-运行：
-
-```bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_orientation_control.py
-```
-
-Expected: FAIL，旧代码仍返回和处理 RPY。
-
-- [x] **Step 2: 实现最少的 NumPy SO(3) 运算**
-
-在现有遥操作模块中加入并只加入实际调用的函数：
-
-```text
-quaternion -> rotation matrix
-rotation matrix -> normalized quaternion
-Log_SO3(rotation) -> 3D rotation vector
-Exp_SO3(rotation vector) -> rotation matrix
-position + rotation -> 4×4 transform
-```
-
-输入必须有限。近似旋转矩阵仅在
-`norm(RᵀR-I) <= 1e-3` 且行列式为正时用 SVD 投影；明显无效输入抛出
-`ValueError`。`Log_SO3` 在接近 `π` 时仍返回最短的有限旋转向量。
-
-- [x] **Step 3: 替换姿态目标、滤波和限速**
-
-控制路径统一为：
-
-```python
-R_xr_delta = R_xr_now @ R_xr_start.T
-R_robot_delta = mapping @ R_xr_delta @ mapping.T
-axis_delta = log_so3(R_robot_delta)
-axis_delta[disabled_axes] = 0.0
-R_raw = exp_so3(axis_delta) @ R_robot_start
-
-error = log_so3(R_target @ R_current.T)
-R_next = exp_so3(scale * error) @ R_current
-```
-
-继续分别保存平移列表和旋转矩阵状态，但构造 QP 目标与调试目标时合成为
-`4×4` 矩阵。删除控制路径中的 `_matrix_to_euler`、
-`_quaternion_to_euler`、分量 `_angle_delta` 及 RPY
-死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。
-
-- [x] **Step 4: 重跑姿态测试**
-
-Expected: PASS。
-
-### Task 4: 把节点状态、QP 和调试话题贯通为 SE(3)
-
-**Files:**
-
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-- Modify: `xr_rm_teleop/test/test_joint_control.py`
-
-- [x] **Step 1: 先将关节控制测试改为 `4×4` 矩阵**
-
-Fake solver 的 `update_joint_state()` 返回有限齐次矩阵；QP
-成功、失败和首帧反馈测试均断言矩阵接口。运行测试，确认旧类型假设失败。
-
-- [x] **Step 2: 完成节点矩阵状态迁移**
-
-- `_robot_start_pose`、`_last_current_pose` 和调试 fallback 改存 `4×4`
- 矩阵。
-- `PlacoIkSolver` 初始化不再接收
- `self._peripheral_config.tool_pose`；外设配置仍只传给
- `RealManAdapter.configure_peripheral()`。
-- 原始目标与发送目标均合成为 `omnipicker_tcp` 的 SE(3)。
-- `TwistStamped.angular` 使用
- `Log(R_sent R_previousᵀ) / dt`，表达在 `rm_base`。
-- `PoseStamped` 只在发布边界把旋转矩阵转四元数。
-- QP 异常继续返回 last-known-good；Grip 松开、超时、反馈错误和发送错误继续
- 走现有慢停与状态重置。
-
-- [x] **Step 3: 运行相关单元测试**
-
-```bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_orientation_control.py \
- src/xr_rm_teleop/test/test_joint_control.py \
- src/xr_rm_teleop/test/test_placo_transforms.py \
- src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-Expected: PASS。
-
-### Task 5: 切换 launch 模型并同步已确认参数
-
-**Files:**
-
-- 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`
-- Modify: `xr_rm_teleop/setup.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-- Modify: `README.md`
-
-- [x] **Step 1: 修改模型来源**
-
-`_rm75_urdf()` 改为：
-
-```python
-PathJoinSubstitution([
- FindPackageShare("xr_rm_teleop"),
- "models",
- "rm75_omnipicker",
- "urdf",
- "RM75-B_OmniPicker_fixed.urdf",
-])
-```
-
-并让 `xr_rm_teleop/setup.py` 安装该目录下的 fixed URDF 和两组 mesh。
-
-- [x] **Step 2: 只修改已确认参数**
-
-节点默认值、launch 默认值和三份 YAML 对应项同步：
-
-```yaml
-control_rate_hz: 125.0
-orientation_deadband_rad: 0.005
-orientation_filter_alpha: 0.65
-max_orientation_speed: 0.5
-follow: false
-```
-
-其中右臂 YAML 的
-`move_to_initial_pose_on_connect: True`
-改为 `false`。不修改任何工作空间、圆柱、线速度、关节速度、初始角、
-`avoid_singularity`、安全配置或外设配置。
-
-- [x] **Step 3: 更新 README 中已失真的运行说明**
-
-只更新：
-
-- 默认控制频率 `90.0 -> 125.0`。
-- QP 模型改为一体化 fixed URDF，并直接控制 `omnipicker_tcp`。
-- 姿态死区、滤波和限速使用 SO(3) 最短路径，不使用 RPY。
-- `peripherals_rm75.yaml` 仍只用于真实控制器工具坐标、负载和外设选择，不再
- 参与 Placo TCP 矩阵换算。
-
-### Task 6: 构建、数值 smoke 与 mock 启动验证
-
-**Files:**
-
-- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
-
-- [x] **Step 1: 构建整个工作空间**
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-```
-
-Expected: `xr_rm_teleop` 和 `xr_rm_bringup` 构建成功。
-
-- [x] **Step 2: 运行指定姿态测试和相关回归测试**
-
-```bash
-source /opt/ros/humble/setup.bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_orientation_control.py
-
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_joint_control.py \
- src/xr_rm_teleop/test/test_placo_transforms.py \
- src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-Expected: PASS。
-
-- [x] **Step 3: 使用固定 XR Python 运行 Placo 数值 smoke**
-
-```bash
-source install/setup.bash
-/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_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf
-```
-
-对左右初始关节姿态分别验证：
-
-- 七个运动关节及顺序正确。
-- `omnipicker_tcp` 相对 `link_7` 为 `[0, 0, 0.16]`、单位旋转。
-- QP 输出七个有限关节角并满足位置与单周期速度限制。
-- 目标停止两秒时打印最大关节变化，但不把漂移设为失败条件。
-- 运动目标最终 TCP 位置误差 `<= 5 mm`，姿态误差 `<= 2°`。
-- 打印平均/最大求解耗时及超过 `8 ms` 周期预算的次数，只记录、不设机器相关
- 的硬失败阈值。
-
-- [x] **Step 4: 只启动 mock**
-
-分别短时启动：
-
-```bash
-source /opt/ros/humble/setup.bash
-source install/setup.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
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-```
-
-确认 fixed URDF、Placo 和左右节点名加载成功，无 RealMan SDK 导入或网络连接。
-由人工结束 mock launch；Codex 不执行任何 `use_mock:=false` 命令。
-
-- [x] **Step 5: 最终范围检查**
-
-```bash
-git diff --check
-git status --short
-git diff -- \
- src/xr_rm_teleop \
- src/xr_rm_bringup \
- src/README.md \
- src/docs/superpowers
-```
-
-确认 `peripherals_rm75.yaml`、`avoid_singularity`、可操作度权重和所有既有安全
-限制未被改变。
diff --git a/docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md b/docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md
deleted file mode 100644
index 9e79065..0000000
--- a/docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md
+++ /dev/null
@@ -1,508 +0,0 @@
-# RM75 CANFD UDP Feedback 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:** Replace synchronous TCP joint polling with the vendor UDP realtime callback while making YAML the source of robot behavior and hardware defaults.
-
-**Architecture:** Keep one `RoboticArm(RM_TRIPLE_MODE_E)` handle per arm. TCP sends CANFD and safety/tool commands; a 5 ms controller UDP push invokes a minimal callback that updates the existing locked joint snapshot. Launch keeps only topology, mock safety mode, PICO input, and generated paths/topics.
-
-**Tech Stack:** Python 3.10, ROS2 Humble, RealMan Python API2, YAML, pytest, colcon
-
----
-
-### Task 1: Add failing UDP feedback adapter tests
-
-**Files:**
-- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
-
-- [ ] **Step 1: Replace polling-specific tests with UDP callback tests**
-
-Add `sys`, `types`, and `SimpleNamespace` imports. Replace
-`test_joint_feedback_is_cached_in_radians` and
-`test_feedback_loop_uses_absolute_schedule_without_catch_up` with helpers and
-tests equivalent to:
-
-```python
-def _udp_state(robot_ip="127.0.0.1", joints=None, error_code=0):
- return SimpleNamespace(
- errCode=error_code,
- arm_ip=robot_ip.encode(),
- joint_status=SimpleNamespace(
- joint_position=joints or [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
- ),
- )
-
-
-def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
- monotonic = iter([10.0, 10.005])
- monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
- adapter = RealManAdapter(
- "127.0.0.1",
- 8080,
- 0,
- "127.0.0.1",
- 8090,
- )
-
- adapter._accept_realtime_feedback = True
- adapter._on_realtime_arm_state(_udp_state())
- first = adapter.get_latest_joint_state()
- adapter._on_realtime_arm_state(_udp_state())
- second = adapter.get_latest_joint_state()
-
- assert first is not None
- assert first.positions == pytest.approx(
- [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
- )
- assert first.read_duration_ms is None
- assert first.update_interval_ms is None
- assert second is not None
- assert second.update_interval_ms == pytest.approx(5.0)
-
-
-@pytest.mark.parametrize(
- "state",
- [
- _udp_state(error_code=-3),
- _udp_state(robot_ip="192.168.192.18"),
- _udp_state(joints=[0.0] * 6),
- _udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
- ],
-)
-def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
- adapter = RealManAdapter(
- "127.0.0.1",
- 8080,
- 0,
- "127.0.0.1",
- 8090,
- )
- adapter._accept_realtime_feedback = True
- adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
- before = adapter.get_latest_joint_state()
-
- adapter._on_realtime_arm_state(state)
-
- assert adapter.get_latest_joint_state() == before
-```
-
-Add a fake vendor module that records callback registration and push config.
-Its `rm_set_realtime_push()` invokes the registered callback with `_udp_state()`.
-Assert:
-
-```python
-adapter.connect()
-arm = fake_module.RoboticArm.instance
-assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
-assert arm.callback is adapter._realtime_callback
-assert adapter.get_latest_joint_state() is not None
-assert not hasattr(adapter, "_feedback_thread")
-```
-
-Add failure cases where `rm_set_realtime_push()` returns `1`, and where
-`adapter._feedback_ready.wait` returns `False`. Both must raise `RuntimeError`;
-the fake arm must record one `rm_delete_robot_arm()` call.
-
-- [ ] **Step 2: Run the focused tests and verify RED**
-
-Run:
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-Expected: FAIL because `RealManAdapter` does not accept realtime push
-parameters and has no `_on_realtime_arm_state`.
-
----
-
-### Task 2: Implement single-handle UDP feedback
-
-**Files:**
-- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
-- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
-- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
-
-- [ ] **Step 1: Replace polling constructor state with realtime push state**
-
-Change `RealManAdapter.__init__` positional parameters from `feedback_period`
-to:
-
-```python
-realtime_push_host_ip: str,
-realtime_push_port: int,
-realtime_push_cycle_ms: int = 5,
-```
-
-Validate with stdlib `ipaddress.IPv4Address`:
-
-```python
-try:
- self._realtime_push_host_ip = str(
- ipaddress.IPv4Address(realtime_push_host_ip)
- )
-except ipaddress.AddressValueError as exc:
- raise ValueError("realtime_push_host_ip must be a valid IPv4 address") from exc
-if not 1 <= realtime_push_port <= 65535:
- raise ValueError("realtime_push_port must be between 1 and 65535")
-if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
- raise ValueError("realtime_push_cycle_ms must be a positive multiple of 5")
-```
-
-Store the port and cycle, then replace feedback thread members with:
-
-```python
-self._feedback_ready = threading.Event()
-self._realtime_callback: Any | None = None
-self._accept_realtime_feedback = False
-self._feedback_fault_logged = False
-```
-
-- [ ] **Step 2: Configure callback and UDP push during connect**
-
-Import these SDK symbols inside `connect()` so mock mode stays SDK-free:
-
-```python
-from Robotic_Arm.rm_robot_interface import (
- RoboticArm,
- rm_realtime_arm_state_callback_ptr,
- rm_realtime_push_config_t,
- rm_thread_mode_e,
-)
-```
-
-After existing safety and optional initial-pose configuration:
-
-```python
-self._feedback_ready.clear()
-self._accept_realtime_feedback = True
-self._realtime_callback = rm_realtime_arm_state_callback_ptr(
- self._on_realtime_arm_state
-)
-self._arm.rm_realtime_arm_state_call_back(self._realtime_callback)
-config = rm_realtime_push_config_t(
- self._realtime_push_cycle_ms,
- True,
- self._realtime_push_port,
- 0,
- self._realtime_push_host_ip,
-)
-self._check_return(
- self._arm.rm_set_realtime_push(config),
- "rm_set_realtime_push",
-)
-if not self._feedback_ready.wait(timeout=2.0):
- raise RuntimeError(
- "RealMan UDP realtime feedback did not receive a valid frame within 2 seconds"
- )
-```
-
-Wrap post-handle initialization so any exception disables callback acceptance,
-deletes the handle, sets `_arm = None`, and re-raises.
-
-- [ ] **Step 3: Implement the bounded callback**
-
-Replace `_feedback_loop()` and `_read_joint_state_once()` with:
-
-```python
-def _on_realtime_arm_state(self, data: Any) -> None:
- if not self._accept_realtime_feedback:
- return
- try:
- if data is None or int(data.errCode) != 0:
- raise ValueError("invalid realtime feedback error code")
- arm_ip = data.arm_ip
- if isinstance(arm_ip, bytes):
- arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
- if str(arm_ip) != self._robot_ip:
- raise ValueError(f"unexpected realtime feedback source: {arm_ip}")
- degrees = list(data.joint_status.joint_position)
- if (
- len(degrees) != 7
- or not all(isinstance(value, Number) for value in degrees)
- ):
- raise ValueError("RM75 UDP feedback must contain 7 numeric joints")
- positions = [math.radians(float(value)) for value in degrees]
- if not all(math.isfinite(value) for value in positions):
- raise ValueError("RM75 UDP feedback contains NaN/Inf")
- received_at = time.monotonic()
- with self._joint_state_lock:
- update_interval_ms = (
- None
- if self._latest_joint_state is None
- else (received_at - self._latest_joint_state.received_at) * 1000.0
- )
- self._latest_joint_state = JointStateSnapshot(
- positions,
- received_at,
- None,
- update_interval_ms,
- )
- self._feedback_fault_logged = False
- self._feedback_ready.set()
- except Exception as exc:
- if not self._feedback_fault_logged:
- self._log_warn(f"RealMan UDP realtime feedback invalid: {exc}")
- self._feedback_fault_logged = True
-```
-
-In `close()`, set `_accept_realtime_feedback = False` before slow-stop and
-handle deletion. Remove feedback thread stop/join logic. Keep the callback
-reference alive until after `rm_delete_robot_arm()`.
-
-- [ ] **Step 4: Declare and pass ROS parameters**
-
-In `SingleArmVelocityTeleop`, declare:
-
-```python
-self.declare_parameter("realtime_push_host_ip", "")
-self.declare_parameter("realtime_push_port", 0)
-self.declare_parameter("realtime_push_cycle_ms", 5)
-```
-
-Replace `feedback_period=self._dt` in `_make_adapter()` with:
-
-```python
-realtime_push_host_ip=str(
- self.get_parameter("realtime_push_host_ip").value
-),
-realtime_push_port=int(
- self.get_parameter("realtime_push_port").value
-),
-realtime_push_cycle_ms=int(
- self.get_parameter("realtime_push_cycle_ms").value
-),
-```
-
-Update all direct `RealManAdapter(...)` calls in tests to pass a host and port.
-
-- [ ] **Step 5: Run focused tests and verify GREEN**
-
-Run:
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
-```
-
-Expected: all focused tests pass, with no real SDK connection.
-
----
-
-### Task 3: Move robot defaults into YAML and simplify launch
-
-**Files:**
-- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
-- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
-- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
-- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
-
-- [ ] **Step 1: Run a failing ownership assertion**
-
-Run a one-off Python assertion that requires the three YAMLs to contain UDP
-and tool parameters, and requires launch not to declare robot behavior
-arguments:
-
-```python
-from pathlib import Path
-import yaml
-
-config_dir = Path("xr_rm_bringup/config")
-for name in ("left_arm_rm75.yaml", "right_arm_rm75.yaml"):
- params = yaml.safe_load((config_dir / name).read_text())
- params = params["single_arm_velocity_teleop"]["ros__parameters"]
- assert "use_mock" not in params
- assert params["realtime_push_host_ip"] == "192.168.192.148"
- assert params["realtime_push_cycle_ms"] == 5
- assert params["enable_tool_control"] is True
-
-source = Path("xr_rm_bringup/launch/arm_debug.launch.py").read_text()
-for name in (
- "left_robot_ip",
- "right_robot_ip",
- "robot_port",
- "avoid_singularity",
- "control_rate_hz",
- "follow",
- "configure_safety_limits",
- "move_to_initial_pose_on_connect",
-):
- assert f'DeclareLaunchArgument("{name}"' not in source
-```
-
-Expected: FAIL because the YAML parameters are missing and launch still
-declares overrides.
-
-- [ ] **Step 2: Update all YAML nodes**
-
-Remove `use_mock`. Add:
-
-```yaml
-realtime_push_host_ip: 192.168.192.148
-realtime_push_cycle_ms: 5
-enable_tool_control: true
-enable_trigger_gripper_control: true
-trigger_close_threshold: 0.95
-configure_peripheral_on_connect: true
-```
-
-Use `realtime_push_port: 8089` for left-arm nodes and `8090` for right-arm
-nodes. Keep:
-
-```yaml
-# all single-arm and dual-arm nodes
-follow: false
-canfd_trajectory_mode: 2
-```
-
-The right-arm high-follow default was reverted after the first hardware test
-exposed an unplanned stationary null-space trajectory. Do not change speeds,
-workspace limits, timeouts, safety limits, or initial pose defaults.
-
-- [ ] **Step 3: Reduce launch overrides**
-
-Make `_single_arm_node(arm, use_mock)` and `_dual_arm_nodes(use_mock)` load
-their YAML first, then pass only:
-
-```python
-{
- "use_mock": use_mock,
- "robot_urdf_path": _rm75_urdf(),
- "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
- "peripheral_arm": arm,
- "tool_command_topic": f"/xr_rm/{_arm_name(arm)}/tool_enable",
-}
-```
-
-Keep equivalent per-side generated values in dual mode. Remove
-`_initial_pose_override`, robot IP/port, avoid-singularity, control-rate,
-follow, safety, tool-control and initial-pose parsing from `_launch_setup`.
-
-Keep only these launch arguments:
-
-```python
-DeclareLaunchArgument("arm", default_value="right")
-DeclareLaunchArgument("use_mock", default_value="true")
-DeclareLaunchArgument("udp_host", default_value="0.0.0.0")
-DeclareLaunchArgument("udp_port", default_value="15000")
-DeclareLaunchArgument("udp_timer_hz", default_value="200.0")
-```
-
-- [ ] **Step 4: Re-run ownership assertion and inspect launch arguments**
-
-Run the assertion from Step 1, then:
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py --show-args
-```
-
-Expected: the assertion passes; launch lists only `arm`, `use_mock`,
-`udp_host`, `udp_port`, and `udp_timer_hz`.
-
----
-
-### Task 4: Synchronize launcher UI and README
-
-**Files:**
-- Modify: `xr_rm_bringup/tools/launcher_ui.py`
-- Modify: `README.md`
-
-- [ ] **Step 1: Remove deleted launch arguments from UI commands**
-
-Keep ping targets unchanged. Change real launch commands to:
-
-```python
-"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false"
-"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false"
-"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false"
-```
-
-- [ ] **Step 2: Update README ownership and commands**
-
-Remove examples and launch-argument descriptions for robot IP/port,
-avoid-singularity, control-rate, follow, safety/tool flags, and initial-pose
-overrides. State that these values live in the selected YAML. Add the UDP
-feedback parameters, host `192.168.192.148`, ports `8089/8090`, 5 ms cycle,
-and the command used after Wi-Fi changes:
-
-```bash
-ip -4 route get 192.168.192.19
-```
-
-Keep `arm`, `use_mock`, and PICO UDP arguments documented as launch
-arguments. Keep the warning that checked-in default `use_mock=true` prevents
-an accidental real connection.
-
-- [ ] **Step 3: Check syntax and stale references**
-
-Run:
-
-```bash
-python3 -m py_compile \
- xr_rm_bringup/launch/arm_debug.launch.py \
- xr_rm_bringup/tools/launcher_ui.py
-rg -n "left_robot_ip:=|right_robot_ip:=|move_to_initial_pose_on_connect:=" \
- README.md xr_rm_bringup/tools/launcher_ui.py
-```
-
-Expected: compilation passes; `rg` returns no stale command-line overrides.
-
----
-
-### Task 5: Full verification
-
-**Files:**
-- Verify all files changed by Tasks 1–4
-
-- [ ] **Step 1: Run teleop tests**
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-```
-
-Expected: all tests pass.
-
-- [ ] **Step 2: Build all workspace packages**
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install --executor sequential
-```
-
-Expected: `xr_rm_interfaces`, `xr_rm_input`, `xr_rm_teleop`, and
-`xr_rm_bringup` all finish successfully.
-
-- [ ] **Step 3: Verify mock launch without vendor hardware**
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-timeout 8s ros2 launch xr_rm_bringup arm_debug.launch.py \
- arm:=right use_mock:=true
-```
-
-Expected: the mock teleop and UDP input nodes start; timeout ends the launch.
-No RealMan SDK connection is attempted.
-
-- [ ] **Step 4: Inspect final diff**
-
-```bash
-cd /home/robot/WS_xr/src
-git diff --check
-git status --short
-git diff --stat
-```
-
-Expected: no whitespace errors and no unrelated files. Do not commit, push,
-or connect to the real robot unless the user explicitly requests it.
diff --git a/docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md b/docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md
deleted file mode 100644
index 54d3c30..0000000
--- a/docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md
+++ /dev/null
@@ -1,42 +0,0 @@
-# RM75 Right-Arm High-Follow YAML Defaults 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:** Make right-arm single-arm debugging default to RealMan high-follow complete passthrough for phase-two testing.
-
-**Architecture:** Change only the existing right-arm YAML parameters. Keep launch, left-arm, dual-arm, speed limits, safety limits, timeouts, and stop behavior unchanged.
-
-**Tech Stack:** ROS2 Humble, YAML, pytest, colcon
-
----
-
-### Task 1: Change right-arm CANFD defaults
-
-**Files:**
-- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
-
-- [ ] **Step 1: Run a failing configuration assertion**
-
-Run a Python YAML assertion requiring `follow is True` and
-`canfd_trajectory_mode == 0`.
-
-Expected: FAIL because the current values are `false` and `2`.
-
-- [ ] **Step 2: Apply the minimal configuration change**
-
-```yaml
-follow: true
-canfd_trajectory_mode: 0
-```
-
-- [ ] **Step 3: Verify configuration and regressions**
-
-Run the same YAML assertion and expect PASS. Then run:
-
-```bash
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-colcon build --symlink-install --executor sequential
-```
-
-Expected: all tests and all four workspace packages pass.
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
deleted file mode 100644
index 1d78901..0000000
--- a/docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md
+++ /dev/null
@@ -1,284 +0,0 @@
-# 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/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md b/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md
deleted file mode 100644
index 894a1a6..0000000
--- a/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md
+++ /dev/null
@@ -1,30 +0,0 @@
-# RM75 控制周期统计设计
-
-## 目标
-
-在不改变控制、QP、安全停止和真机通信行为的前提下，确认激活遥操作时的
-125 Hz 控制链路是否满足 `8 ms` 周期。
-
-## 方案
-
-在 `SingleArmVelocityTeleop` 内使用 `time.perf_counter_ns()` 采样，仅在
-Grip 激活并执行关节命令的周期记录：
-
-- 相邻控制回调的实际周期；
-- 控制回调总执行时间；
-- Placo QP 求解时间；
-- `send_joint_target()` 调用时间；
-- 当前关节反馈年龄。
-
-每累计约 5 秒激活样本，通过现有 ROS logger 输出一次汇总并清空窗口。每项
-输出样本数、mean、P95、P99 和 max；实际周期与总执行时间额外输出超过
-`self._dt` 的次数。首个激活周期没有可靠的相邻周期值，因此不记录周期。
-
-统计只输出日志，不新增 ROS 消息、话题、参数、依赖或后台线程。计时与日志
-异常不得影响控制路径。
-
-## 验证
-
-先添加一个小单元测试，使用确定性样本验证百分位数、超限计数、日志输出和
-窗口清空。随后运行相关 pytest、姿态控制测试和
-`colcon build --symlink-install`。
diff --git a/docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md b/docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md
deleted file mode 100644
index 1143121..0000000
--- a/docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md
+++ /dev/null
@@ -1,131 +0,0 @@
-# RM75 反馈调度与跟随速度优化设计
-
-## 目标
-
-在保留 Placo 单步 QP、工作空间与圆柱限位、关节速度限制、指令超时和安全
-停止的前提下，分阶段解决 PICO 遥操机械臂跟随速度很慢的问题。
-
-每阶段只改变一个控制因素，真机验证通过后才进入下一阶段：
-
-1. 提高关节反馈的新鲜度；
-2. 启用 `rm_movej_canfd` 高跟随完全透传；
-3. 将右臂单独调试 TCP 速度提高到 `0.2 m/s`。
-
-## 真机证据
-
-右臂在 125 Hz、低跟随模式下连续四个约 5 秒窗口的结果为：
-
-- `period mean=8.000 ms`，四个窗口最大值为 `9.023–9.424 ms`；
-- `total mean=1.908–2.004 ms`，最大值不超过 `4.109 ms`；
-- `qp mean=0.230–0.245 ms`；
-- `send mean=0.122–0.127 ms`；
-- `feedback_read mean=9.101–9.630 ms`；
-- `feedback_interval mean=17.368–17.918 ms`；
-- `feedback_age mean=9.079–9.632 ms`，最差达到 `46.480 ms`。
-
-每个窗口只有 `279–288` 次新反馈，即实际反馈频率约为 `56–58 Hz`。
-`feedback_interval - feedback_read` 在四个窗口中稳定为 `8.27–8.39 ms`，
-确认当前反馈线程把一次 SDK 查询耗时和完整的 8 ms 等待串联起来：
-
-```text
-当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待
-```
-
-控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。
-
-## 阶段一：反馈线程绝对周期调度
-
-### 调度语义
-
-将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：
-
-```text
-读取耗时 < 8 ms：只等待剩余时间
-读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准
-```
-
-调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取，
-不会为了追赶多个旧截止点而密集补调用 SDK。
-
-等价的目标启动间隔为：
-
-```text
-max(8 ms, 本次反馈读取耗时)
-```
-
-当前读取平均约 9.3 ms，因此预期反馈频率接近 `100 Hz`，但不强求达到
-`125 Hz`。
-
-### 范围
-
-本阶段只修改 `RealManAdapter._feedback_loop()` 的等待计算。以下内容保持不变：
-
-- `follow=false`；
-- `canfd_trajectory_mode=2`；
-- 右臂单独调试 `max_linear_speed=0.15 m/s`；
-- 125 Hz ROS 控制定时器和 Placo QP；
-- 同一个 RealMan 连接承担反馈与发送，不新增连接；
-- 所有安全限位、超时和停止逻辑。
-
-现有 `feedback_read`、`feedback_interval` 和其他 timing 指标继续保留。
-
-### 验收
-
-右臂真机连续采集四个 timing 窗口，全部满足：
-
-- `feedback_interval mean ≤ 12 ms`；
-- `feedback_interval mean - feedback_read mean ≤ 2 ms`；
-- `feedback_age mean ≤ 7 ms`；
-- `period max < 10 ms`；
-- `total max < 8 ms`；
-- 无反馈超时、异常停止或 SDK 发送错误。
-
-若更密集的反馈查询使 `period max` 达到或超过 10 ms，或明显增加发送耗时，
-停止后续高跟随阶段，继续定位同一 SDK 连接的读写竞争。
-
-## 阶段二：高跟随完全透传
-
-只有阶段一通过后才验证：
-
-- `follow=true`；
-- `canfd_trajectory_mode=0`；
-- 右臂单独调试速度仍为 `0.15 m/s`。
-
-先通过现有 launch 参数显式启用高跟随完成右臂单机验证；验证通过后，再把
-`arm_debug.launch.py` 默认值和 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml`、
-`right_arm_rm75.yaml` 同步为高跟随完全透传。
-
-验收条件：
-
-- 快速移动手柄约 10 cm 后，机械臂追赶不超过 1 秒；
-- 连续四个 timing 窗口 `period max < 10 ms`；
-- 无明显振荡、跳动、反馈超时或异常停止。
-
-若仍追赶超过 1 秒，不进入加速阶段；先增加关节目标与实测关节误差统计，
-确认慢速来自 QP 单步目标还是控制器执行。
-
-## 阶段三：右臂速度提高到 0.2 m/s
-
-只有阶段二通过后，将 `right_arm_rm75.yaml` 中右臂单独调试的
-`max_linear_speed` 从 `0.15` 提高到 `0.2 m/s`。
-
-- `dual_arm_rm75.yaml` 的左右臂已经是 `0.2 m/s`，无需修改；
-- 左臂单独调试速度保持现状；
-- 右臂真机 `max_line_speed=0.25 m/s` 安全上限保持不变。
-
-右臂 `scale=0.7` 时，手柄移动 10 cm 对应约 7 cm TCP 目标，理论限速时间约
-0.35 秒。验收追赶时间不超过 0.7 秒，并确认没有明显振荡或限位异常。
-
-## 测试与交付
-
-阶段一实现采用测试先行：
-
-- 用确定性时钟和停止事件验证短读取只等待剩余时间；
-- 验证读取超期后不额外等待，也不补跑多个历史周期；
-- 运行 `xr_rm_teleop` 全部 pytest；
-- 运行 `test_orientation_control.py`；
-- 在 `/home/robot/WS_xr` 运行 `colcon build --symlink-install`。
-
-Codex 不连接真机、不移动机械臂。每个阶段的真机验证由用户通过
-`xr_rm_bringup/launch/arm_debug.launch.py` 在右臂、小范围动作下完成，并把连续
-四个完整 timing 窗口返回后再进入下一阶段。
diff --git a/docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md b/docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md
deleted file mode 100644
index 5b3aa59..0000000
--- a/docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md
+++ /dev/null
@@ -1,53 +0,0 @@
-# RM75 关节反馈线程计时统计设计
-
-## 目标
-
-在不改变关节反馈轮询、QP、关节指令和安全停止行为的前提下，测清当前
-RealMan 反馈线程的两个关键时间：
-
-- `rm_get_joint_degree()` 单次调用耗时；
-- 相邻两次成功写入关节反馈缓存的实际更新间隔。
-
-本轮只增加统计。高跟随、TCP 速度、反馈调度和 QP 控制方式均保持现状，待
-真机日志确认根因后再修改。
-
-## 方案
-
-`RealManAdapter` 在 `_read_joint_state_once()` 中使用单调高精度时钟记录：
-
-- `feedback_read`：从调用 `rm_get_joint_degree()` 前到调用返回后的耗时；
-- `feedback_interval`：本次成功反馈时间戳与上次成功反馈时间戳之差。
-
-两个数值随 `JointStateSnapshot` 写入现有线程安全缓存。首次成功反馈没有可靠
-的前序时间戳，因此不提供 `feedback_interval`。
-
-`SingleArmVelocityTeleop` 只在看到新的反馈时间戳时，将这两个数值各记录一次，
-避免 125 Hz 控制循环重复读取同一缓存而造成重复统计。统计加入现有约 5 秒
-timing 窗口，并输出各自的样本数、mean、P95、P99 和 max：
-
-```text
-feedback_read[n=<样本数> mean=<均值> p95= p99= max=<最大值> ms]
-feedback_interval[n=<样本数> mean=<均值> p95= p99= max=<最大值> ms]
-```
-
-读取失败不产生成功样本，继续沿用现有一次告警、反馈超时和安全停止逻辑。
-Mock 模式不伪造厂商 API 调用耗时。
-
-## 验证
-
-- 扩展现有关节控制单元测试，使用确定性快照验证新反馈只统计一次、重复缓存
- 不重复计数、首次反馈没有更新间隔。
-- 运行 `xr_rm_teleop` 相关 pytest。
-- 按项目规则在工作空间根目录运行 `colcon build --symlink-install`。
-- 真机测试仍由用户使用 `arm_debug.launch.py arm:=right use_mock:=false` 执行；
- 本轮不自动连接机械臂。
-
-## 后续决策
-
-用户提供真机 timing 日志后再判断：
-
-- 若 `feedback_interval` 主要由 `feedback_read + 8 ms` 构成，再评估绝对周期
- 调度；
-- 若 `feedback_read` 本身经常超过 8 ms，优先定位厂商查询或同一连接的读写
- 竞争；
-- 反馈问题确认前，不把高跟随或预测式 QP 与本轮统计改动混在一起。
diff --git a/docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md b/docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md
deleted file mode 100644
index 2427313..0000000
--- a/docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md
+++ /dev/null
@@ -1,61 +0,0 @@
-# RM75 工具坐标系幂等配置设计
-
-## 目标
-
-修复遥操作节点每次启动都无条件创建 RealMan 工具坐标系、忽略重复名称错误，
-随后仍误报“外设配置完成”的问题。
-
-本修复只处理控制器工具坐标系的创建、更新、切换和返回值检查，不修改夹爪
-IO、Modbus、Placo、URDF 或任何机械臂运动控制参数。
-
-## 根因
-
-右臂 `scissorgripper: 1` 选择 `peripherals_rm75.yaml` 中的 `omnipic`。
-`configure_peripheral_on_connect` 默认为 `true`，因此节点每次启动都会进入
-`peripheral_cfg()`。
-
-当前实现无条件执行：
-
-```python
-robot.rm_set_manual_tool_frame(frame=tool_frame)
-robot.rm_change_tool_frame(tool_name)
-```
-
-RealMan 控制器会持久保存工具坐标系。首次启动创建成功，后续启动因
-`omnipic` 已存在而创建失败。两个返回值均未检查，因此代码继续执行并输出
-配置成功日志；若 YAML 中的 TCP、重量或重心已变化，控制器仍可能保留旧值。
-
-## 方案
-
-在 `fun_peripheral.py` 中增加一个小型内部函数，负责单一工具坐标系的幂等
-配置：
-
-1. 调用 `rm_get_total_tool_frame()` 获取现有工具坐标系名称并检查
- `return_code`。
-2. 若目标名称不存在，调用 `rm_set_manual_tool_frame()`。
-3. 若目标名称已存在，调用 `rm_update_tool_frame()`。
-4. 检查创建或更新返回值。
-5. 调用 `rm_change_tool_frame()` 并检查返回值。
-
-任一步失败都抛出包含 SDK 操作名称和返回码的 `RuntimeError`。异常沿现有
-节点初始化链路向上传播，因此不会继续误报“外设配置完成”。
-
-不通过“先删除再创建”实现更新，避免在切换中的控制器上产生短暂无工具
-坐标系状态。
-
-## 测试
-
-在现有外设相关测试文件中使用 FakeArm 覆盖：
-
-- 名称不存在时只调用创建，然后切换；
-- 名称存在时只调用更新，然后切换；
-- 查询、创建/更新或切换失败时抛出明确错误。
-
-随后运行：
-
-- `xr_rm_teleop` 全部 pytest；
-- `test_orientation_control.py`；
-- `/home/robot/WS_xr` 下的 `colcon build --symlink-install`。
-
-Codex 不连接真机。真机验证由用户重新启动右臂 launch，确认不再出现
-`Failed to create the tool frame system`，并能看到外设配置完成日志。
diff --git a/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md b/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md
deleted file mode 100644
index e860b80..0000000
--- a/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md
+++ /dev/null
@@ -1,241 +0,0 @@
-# RM75 SO(3) 姿态跟随与 OmniPicker 一体化模型设计
-
-日期：2026-07-28
-状态：已确认
-
-## 1. 背景与目标
-
-当前遥操作链路已经使用 Placo QP 将 TCP 目标转换为 RM75 七关节目标，但姿态目标在进入 QP 前会转换为 RPY，并按三个欧拉角分量进行滤波和限速。RM75 当前姿态接近俯仰角 `±90°` 时，同一个物理姿态可能切换到另一组等价 RPY，导致控制器把很小的手柄旋转解释为大角度路径，出现机械臂绕行和跟随时间过长。
-
-本设计是
-`2026-07-27-rm75-placo-qp-ik-design.md`
-的增量修改；它覆盖旧设计中的模型、TCP 变换、姿态表示、控制频率和相关验收
-内容。现有的关节反馈、单步 QP、`rm_movej_canfd`、last-known-good 和停止链路
-继续保留。
-
-本次修改的目标是：
-
-1. 姿态控制全程使用旋转矩阵和 SE(3)，按 SO(3) 最短旋转路径滤波和限速。
-2. 左右臂统一使用用户提供的 `RM75-B_OmniPicker_fixed.urdf`。
-3. 在 URDF 内定义 `omnipicker_tcp`，取消 Placo 中重复的运行时末端变换。
-4. 首轮真机测试继续使用低跟随，将控制频率设为 `125 Hz`，TCP 目标角速度上限统一为 `0.5 rad/s`。
-5. 保留现有工作空间、圆柱、关节速度、指令超时和安全停止行为。
-
-## 2. 本次不处理的内容
-
-- 不开启高跟随，`follow` 继续为 `false`。
-- 不改变现有 Wi-Fi/有线混合网络拓扑。
-- 不新增高跟随周期看门狗。
-- 不修改 `avoid_singularity`，不新增奇异点检测或降速策略。
-- 首轮不删除或调整现有可操作度优化任务。
-- 不修改 `peripherals_rm75.yaml` 中的夹爪选择、工具位姿或负载。
-- 不新增 ROS2 节点、消息、求解器工厂或第三方依赖。
-- Codex 不连接或移动真实机械臂和夹爪。
-
-## 3. 姿态数据模型
-
-控制路径中的机器人位姿统一为有限的 `4×4` NumPy SE(3) 矩阵：
-
-```text
-T = [ R p ]
- [ 0 1 ]
-```
-
-其中 `R` 为 `3×3` 旋转矩阵，`p` 为 TCP 在机器人基坐标系下的位置。Placo 正解直接返回该矩阵，QP 目标也直接接收该矩阵。现有仅保存 `x/y/z/rx/ry/rz` 的 `ArmPose` 不再作为控制路径接口，避免在遥操作节点与 Placo 之间发生 RPY 往返转换。
-
-ROS 调试边界按消息类型转换：
-
-- `PoseStamped`：旋转矩阵转换为四元数后发布。
-- `TwistStamped.angular`：发布相邻两个目标旋转之间、在机器人基坐标系表达的 SO(3) 旋转向量速度。
-
-## 4. XR 姿态到机器人姿态
-
-Grip 按下时保存 XR 初始四元数和当前 `omnipicker_tcp` 初始旋转矩阵。每周期计算：
-
-```text
-R_xr_delta = R_xr_now * transpose(R_xr_start)
-R_robot_delta = M * R_xr_delta * transpose(M)
-R_raw_target = R_robot_delta * R_robot_start
-```
-
-`M` 为现有 `xr_to_robot_matrix`，其左右臂映射保持不变。`enable_orientation_axes` 继续保留；被关闭的机器人基坐标轴通过将对应 SO(3) 相对旋转向量分量置零实现，不再通过拼接 RPY 分量实现。
-
-姿态处理统一使用基坐标系下的左乘增量：
-
-```text
-R_error = R_target * transpose(R_current)
-r = Log(R_error)
-R_next = Exp(scale * r) * R_current
-```
-
-处理顺序为：
-
-1. 以 `norm(Log(R_target * R_lastᵀ))` 判断 `orientation_deadband_rad`。
-2. 以 `orientation_filter_alpha` 缩放从滤波状态到目标的最短旋转向量。
-3. 将从上一发送姿态到滤波姿态的旋转角限制在
- `max_orientation_speed / control_rate_hz` 以内。
-4. 将限速后的旋转和已通过现有工作空间限制的位置合成为 SE(3)。
-
-该路径不使用 RPY 展开、分量插值或分量限速。相对旋转接近 `π` 时也必须选择物理最短路径；四元数 `q` 与 `-q` 必须得到相同目标。
-
-位姿误差采用与 Placo `FrameTask` 一致的解耦 `3+3` 形式：
-
-```text
-e_position = p_target - p_current
-e_orientation = Log(R_target * transpose(R_current))
-e_pose = [e_position; e_orientation]
-```
-
-其中 `e_pose` 可视为六维任务误差，但不是
-`Log_SE3(T_target * inverse(T_current))`。本次不引入完整 SE(3) 对数映射中的
-平移—旋转耦合。
-
-## 5. URDF 与 TCP
-
-用户上传的
-`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
-作为模型来源。将 fixed URDF 和被引用的 RM75、OmniPicker mesh 放入
-`xr_rm_teleop/models/rm75_omnipicker`，由现有 `xr_rm_teleop` 包安装。
-
-在 `RM75-B_OmniPicker_fixed.urdf` 中增加：
-
-```xml
-
-
-
-
-
-
-
-```
-
-该变换表示 TCP 相对 RM75 法兰坐标系沿 `+Z` 方向平移 `0.16 m`、坐标轴方向不变。上传模型中 `rm75_flange` 到 `omnipicker_base_link` 为零固定变换，因此上述定义与已确认的法兰到 TCP 变换一致。
-
-`arm_debug.launch.py` 的左臂、右臂和双臂节点统一加载这一份 fixed URDF。模型仍只有 `joint_1` 至 `joint_7` 七个运动关节，OmniPicker 关节均保持 fixed。
-
-## 6. Placo QP
-
-`PlacoIkSolver` 改为：
-
-```text
-构造参数：URDF 路径、dt
-当前位姿：get_T_world_frame("omnipicker_tcp")
-目标任务：add_frame_task("omnipicker_tcp", target_se3)
-```
-
-删除仅供 Placo 使用的 `tool_pose` 构造参数、`_tool_transform`、
-`_tool_inverse` 和对应的法兰/TCP换算函数。`peripherals_rm75.yaml` 保持原状，
-仍供 `RealManAdapter` 配置真实控制器的工具坐标、负载和末端外设；其中的
-`pose` 不再传入 Placo，因此不会在 QP 中重复叠加末端偏移。
-
-QP 配置首轮保持：
-
-```text
-frame task: soft, 1.0
-manipulability task: soft, 5e-2
-kinetic energy regularizer: 1e-6
-```
-
-可操作度任务已知会造成目标静止时的关节姿态变化，但按用户决定本轮保留，
-待确认 RPY 绕行消失后再单独评估。关节位置和单周期速度校验继续使用 URDF
-限制，固定虚拟基座和 `q[7:14]` 的七关节映射保持不变。
-
-## 7. 参数与启动行为
-
-以下配置在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
-`dual_arm_rm75.yaml` 对应节点中同步：
-
-```yaml
-control_rate_hz: 125.0
-orientation_deadband_rad: 0.005
-orientation_filter_alpha: 0.65
-max_orientation_speed: 0.5
-follow: false
-```
-
-`arm_debug.launch.py` 的 `control_rate_hz` 默认值同步为 `125.0`，`follow`
-默认值继续为 `false`。`single_arm_velocity_teleop` 内部参数默认值同步，
-避免绕过 YAML 启动时回到旧频率或旧角速度。
-
-`right_arm_rm75.yaml` 的 `move_to_initial_pose_on_connect` 从 `True` 改为
-`false`，与左臂、双臂以及 launch 默认安全行为一致。其他左右臂空间范围、
-线速度、关节速度、加速度、初始关节角和外设配置不变。
-
-## 8. 异常与停止
-
-现有异常策略保持：
-
-- 非法 XR 四元数、Grip 松开、XR 超时、关节反馈过期或通信失败时执行现有慢停止并重置激活状态。
-- QP 失败或输出违反关节位置/速度限制时继续使用上一组有效关节目标。
-- 第一次有效关节反馈前不发送运动命令。
-- `configure_safety_limits` 保持启用。
-- Mock 模式不导入睿尔曼 SDK。
-
-新增 SO(3) 运算必须拒绝非有限矩阵和零四元数。旋转矩阵若满足
-`norm(RᵀR-I) <= 1e-3` 且行列式为正，则使用 `3×3` SVD 投影到最近的合法旋转；
-超出该范围时停止输出，不能把明显无效的输入静默修正成运动目标。
-
-## 9. 验证
-
-### 9.1 自动测试
-
-扩展 `test_orientation_control.py`，至少覆盖：
-
-- 初始姿态俯仰接近 `+90°` 和 `-90°` 时，小手柄旋转仍产生相同量级的最短物理旋转。
-- 目标跨越原 RPY 表示分支时，不产生接近 `π` 的错误路径。
-- `q` 与 `-q` 产生相同旋转矩阵。
-- SO(3) 死区使用整体旋转角。
-- 每周期姿态步长不超过 `0.5 / 125 rad`。
-- SO(3) 滤波沿最短路径收敛。
-- 调试四元数有限且归一化。
-
-更新 Placo 变换测试和 smoke test，验证：
-
-- fixed URDF 可加载，运动关节仍严格为七个且顺序正确。
-- `omnipicker_tcp` 相对法兰的变换为 `[0, 0, 0.16]` 和单位旋转。
-- 当前/原始目标/发送目标均表示 `omnipicker_tcp`。
-- 一次 QP 输出七个有限关节角并满足位置与单周期速度限制。
-- 目标静止两秒时记录左右臂最大关节变化，但本轮不以 `≤0.5°` 作为通过条件。
-- 目标停止后 TCP 位置误差不超过 `5 mm`、姿态误差不超过 `2°`。
-
-### 9.2 命令
-
-所有命令从 `/home/robot/WS_xr` 执行：
-
-```bash
-source /opt/ros/humble/setup.bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
- src/xr_rm_teleop/test/test_orientation_control.py
-
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-
-source /opt/ros/humble/setup.bash
-source install/setup.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
-```
-
-Placo smoke test继续使用已验证的
-`/home/robot/miniconda3/envs/xr/bin/python`，周期改为 `1/125 s`。
-
-### 9.3 真机人工验收
-
-Codex 只提供步骤，不执行真机操作。用户应分别测试左右臂：
-
-1. 确认急停可用、周围无障碍物且 `move_to_initial_pose_on_connect=false`。
-2. 先保持手柄和目标姿态不变，记录 TCP 与关节变化。
-3. 仅改变手柄姿态，重点跨越原俯仰 `±90°` 附近的 RPY 分支。
-4. 确认 TCP 以最短物理旋转跟随，没有绕一大圈。
-5. 确认 `omnipicker_tcp` 位置保持在允许误差内。
-
-## 10. 完成标准
-
-1. 控制路径中的目标生成、滤波、限速和 QP 接口不再使用 RPY。
-2. 左右臂都由同一 fixed URDF 的 `omnipicker_tcp` 作为控制帧。
-3. Placo 不再使用 `peripherals_rm75.yaml` 的工具位姿进行矩阵换算。
-4. 可操作度任务保持现状，漂移数据被记录但不作为首轮阻断项。
-5. 三份配置使用 `125 Hz`、`0.5 rad/s` 和低跟随。
-6. 右臂连接时不再自动移动到初始关节姿态。
-7. 指定测试、构建和左右臂 mock 启动验证通过。
-8. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
diff --git a/docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md b/docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md
deleted file mode 100644
index f60a2d7..0000000
--- a/docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md
+++ /dev/null
@@ -1,198 +0,0 @@
-# RM75 CANFD UDP 主动反馈设计
-
-> 真机验证修正：右臂高跟随首轮测试触发掉使能。离线复现发现静止目标存在
-> `23.582°` 空空间漂移，第一周期约 `3315°/s²`。当前实现已移除
-> manipulability 自运动、增加软件关节加速度限幅和故障 Grip 锁存，并将三份
-> YAML 恢复为 `follow: false` 安全基线；高跟随须在基线验证后单独测试。
-
-## 目标
-
-按睿尔曼 MovejCANFD 示例，将真机关节反馈从同一 TCP 控制连接上的
-`rm_get_joint_degree()` 周期轮询，替换为控制器 UDP 主动状态推送。
-
-控制命令继续通过现有单个 `RoboticArm(RM_TRIPLE_MODE_E)` 句柄发送，不新增
-RealMan 连接，不修改 Placo QP、工作空间/圆柱限位、速度限制、指令超时或安全
-停止条件。
-
-## 根因与证据
-
-低跟随模式下，125 Hz CANFD 发送与绝对周期 TCP 反馈轮询能够同时工作：
-
-- `feedback_interval mean=10.041–10.389 ms`；
-- `feedback_age mean=5.730–6.138 ms`；
-- 控制周期最大值不超过 `8.979 ms`。
-
-启用高跟随后，即使 `canfd_trajectory_mode=2`，控制发送仍正常：
-
-- `period max=9.275 ms`；
-- `send max=0.235 ms`。
-
-但同步反馈退化为：
-
-- `feedback_read mean=11.278 ms`、`max=80.320 ms`；
-- `feedback_age max=117.502 ms`。
-
-反馈年龄逼近现有 `command_timeout_sec=0.12 s`，触发“关节反馈缺失或过期”
-安全停止，造成 Grip 按住期间控制反复退出和重新锁定。增大超时只会允许 QP
-继续使用更旧的关节状态，不解决 TCP 反馈阻塞。
-
-睿尔曼 MovejCANFD 示例使用三线程模式、`rm_set_realtime_push()` 和
-`rm_realtime_arm_state_call_back()`，通过 UDP 回调获取关节状态，而不是在
-CANFD 透传期间同步轮询关节角。
-
-## 数据流
-
-```text
-PICO -> ROS 125 Hz 控制回调 -> Placo QP -> TCP rm_movej_canfd
-
-RM75 控制器 -> UDP 5 ms 主动推送 -> SDK 第三线程回调
- -> JointStateSnapshot 缓存 -> ROS 125 Hz 控制回调
-```
-
-TCP 仍承担 CANFD、慢停、安全配置和末端工具命令。UDP 只承担状态反馈，两条
-传输路径共用同一个机械臂句柄。
-
-## SDK 连接与反馈生命周期
-
-`RealManAdapter.connect()` 保持三线程模式和单次 `rm_create_robot_arm()`：
-
-1. 创建并检查机械臂句柄。
-2. 下发已有安全参数和可选初始位姿。
-3. 创建并保存 `rm_realtime_arm_state_callback_ptr`，避免 Python 回调被垃圾
- 回收。
-4. 使用 `rm_realtime_push_config_t` 配置 5 ms UDP 主动上报。
-5. 注册 `rm_realtime_arm_state_call_back()`。
-6. 等待第一帧有效 UDP 反馈，最长 2 秒。
-
-若配置接口返回非零，或 2 秒内没有有效反馈，连接初始化失败并删除已创建的
-机械臂句柄；不静默回退到 TCP 轮询。
-
-连接成功后不再创建反馈线程，也不再调用 `rm_get_joint_degree()`。
-`close()` 先停止接受回调更新，再执行现有慢停和句柄删除。控制器的 UDP 配置
-由下一次启动重新覆盖，不额外增加关闭阶段配置命令。
-
-## UDP 回调与缓存
-
-回调只执行有界、非阻塞工作：
-
-1. 检查回调对象、`errCode` 和来源机械臂 IP。
-2. 读取 7 个 `joint_position`，检查数量、数值类型及 NaN/Inf。
-3. 将厂商反馈的角度转换为弧度。
-4. 使用 `time.monotonic()` 记录接收时刻，并计算与上一帧的更新间隔。
-5. 在现有 `_joint_state_lock` 下替换 `JointStateSnapshot`。
-6. 第一帧有效数据唤醒连接初始化等待。
-
-无效 UDP 帧不覆盖上一帧缓存。若后续持续丢包，现有 120 ms 新鲜度检查自然
-触发安全停止。
-
-UDP 回调不执行 QP、ROS 发布、停止命令或其他 SDK 调用，避免阻塞 SDK 接收
-线程。
-
-## 参数与三份 YAML
-
-新增真机参数：
-
-- `realtime_push_host_ip`：机械臂可直接访问的上位机地址；
-- `realtime_push_port`：单臂 UDP 接收端口；
-- `realtime_push_cycle_ms`：主动上报周期，默认并配置为 `5`。
-
-本次现场配置：
-
-| 配置 | 节点 | host | port |
-|---|---|---|---:|
-| `right_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
-| `left_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
-| `dual_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
-| `dual_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
-
-左右臂端口必须不同。更换上位机或网络后，只需同步修改 YAML 中的
-`realtime_push_host_ip`。
-
-Mock 模式不导入厂商 SDK，也不要求 UDP 参数有效。
-
-## YAML 与 launch 参数所有权
-
-此前 `arm_debug.launch.py` 会用 launch 默认值覆盖 YAML 中的机械臂参数，
-导致 YAML 无法单独控制 `follow` 等行为。
-
-用户选择由 YAML 作为机械臂行为和硬件参数的唯一默认来源。
-
-以下参数只由 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
-`dual_arm_rm75.yaml` 管理，launch 不再声明或覆盖：
-
-- `robot_ip`、`robot_port`；
-- `avoid_singularity`；
-- `control_rate_hz`；
-- `follow`、`canfd_trajectory_mode`、`canfd_radio`；
-- `configure_safety_limits`；
-- `move_to_initial_pose_on_connect`；
-- `enable_tool_control`、`enable_trigger_gripper_control`；
-- `trigger_close_threshold`；
-- `configure_peripheral_on_connect`；
-- 本设计新增的 UDP 主动反馈参数。
-
-三份 YAML 补齐工具控制参数；删除其中不再生效的 `use_mock`，避免出现两个
-配置来源。
-
-`arm_debug.launch.py` 只保留：
-
-- `arm=left|right|both`，选择启动拓扑；
-- `use_mock=true|false`，作为显式安全运行模式，默认仍为 `true`；
-- PICO 输入节点的 `udp_host`、`udp_port`、`udp_timer_hz`；
-- launch 根据安装路径和左右臂生成的 `robot_urdf_path`、
- `peripheral_config_file`、`peripheral_arm` 和 `tool_command_topic`。
-
-`launcher_ui.py` 和 README 中的启动命令同步删除机械臂 IP、初始化移动等已
-移交 YAML 的 launch 参数，只保留 `arm`、`use_mock` 和 PICO 输入覆盖。
-
-控制模式保持分阶段范围：
-
-- 三份 YAML 均使用 `follow: false`、`canfd_trajectory_mode: 2` 完成安全基线；
-- 基线验证通过前不启用高跟随或模式 0，不提高 `max_linear_speed`。
-
-## Timing 日志
-
-保留：
-
-- `period`；
-- `total`；
-- `qp`；
-- `send`；
-- `feedback_age`；
-- `feedback_interval`。
-
-`feedback_read` 表示同步 SDK 查询耗时；UDP 架构不存在该查询，因此该字段
-不再产生样本，现有条件日志逻辑会自动省略它，不新增同义统计项。
-
-## 测试
-
-使用 FakeArm 和伪造 SDK 模块覆盖：
-
-- 连接时使用正确 host、port、5 ms 周期配置 UDP 并注册回调；
-- 第一帧有效回调转换 7 个关节角为弧度并解除启动等待；
-- 连续回调正确记录 `feedback_interval`；
-- 错误码、来源 IP、长度或 NaN/Inf 无效帧不覆盖缓存；
-- UDP 配置失败或首帧超时会清理句柄并抛出明确异常；
-- 真机适配器不再启动轮询线程或调用 `rm_get_joint_degree()`；
-- Mock 模式无需厂商 SDK。
-- launch 不再覆盖 YAML 的机械臂行为和硬件参数；
-- `use_mock` 仍由 launch 默认设为 `true`；
-- `launcher_ui.py` 不再传递已经删除的 launch 参数。
-
-随后运行：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-colcon build --symlink-install --executor sequential
-```
-
-Codex 不连接真机。用户在右臂小范围测试中确认：
-
-- 启动日志显示收到 UDP 首帧；
-- 按住 Grip 不再出现反馈过期或 SDK `-2`；
-- 连续四个 timing 窗口 `period max < 10 ms`、`total max < 8 ms`；
-- `feedback_interval mean` 接近 5 ms；
-- `feedback_age mean < 5 ms`，且最大值不触发 120 ms 安全停止。