feat: add RM75 Placo single-step QP inverse kinematics design document

docs/superpowers/specs/2026-07-27-rm75-placo-qp-ik-design.md

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