From f795c06d4406af9219be907df615e53c428731ba Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Wed, 29 Jul 2026 16:24:07 +0800 Subject: [PATCH] Remove outdated design documents for RM75 control and feedback systems --- AGENTS.md | 4 +- .../plans/2026-07-27-rm75-placo-qp-ik.md | 1644 ----------------- .../2026-07-28-rm75-control-timing-stats.md | 154 -- ...07-28-rm75-feedback-absolute-scheduling.md | 109 -- .../2026-07-28-rm75-feedback-thread-timing.md | 167 -- .../2026-07-28-rm75-idempotent-tool-frame.md | 107 -- .../2026-07-28-rm75-so3-omnipicker-teleop.md | 390 ---- .../2026-07-29-rm75-canfd-udp-feedback.md | 508 ----- ...26-07-29-rm75-high-follow-yaml-defaults.md | 42 - .../2026-07-27-rm75-placo-qp-ik-design.md | 284 --- ...-07-28-rm75-control-timing-stats-design.md | 30 - ...feedback-scheduling-follow-speed-design.md | 131 -- ...7-28-rm75-feedback-thread-timing-design.md | 53 - ...07-28-rm75-idempotent-tool-frame-design.md | 61 - ...07-28-rm75-so3-omnipicker-teleop-design.md | 241 --- ...26-07-29-rm75-canfd-udp-feedback-design.md | 198 -- 16 files changed, 3 insertions(+), 4120 deletions(-) delete mode 100644 docs/superpowers/plans/2026-07-27-rm75-placo-qp-ik.md delete mode 100644 docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md delete mode 100644 docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md delete mode 100644 docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md delete mode 100644 docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md delete mode 100644 docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md delete mode 100644 docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md delete mode 100644 docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md delete mode 100644 docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md delete mode 100644 docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md delete mode 100644 docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md delete mode 100644 docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md delete mode 100644 docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md delete mode 100644 docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md delete mode 100644 docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md 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 安全停止。