# 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，未升级或全局安装
   数值依赖。