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

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。启动链路为：

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 范围一致：

J1 ±178°, J2 ±130°, J3 ±178°, J4 ±135°,
J5 ±178°, J6 ±128°, J7 ±360°

旧的、当前未被调用的 fun_peripheral.alg_init() 自定义限位不作为 QP 限位来源。控制器侧现有 configure_safety_limits、关节最大速度和最大加速度设置继续保留。

参考：

• 睿尔曼 RM75-B 本体参数
• XRoboToolkit DualArmURController

6. 工具 TCP 处理

URDF 只描述到 link_7，实际工具来自 peripherals_rm75.yaml。同一份工具配置有两个使用者：

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。

正解和目标换算为：

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 真机示例的最小配置：

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 数据流

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。

• 运行现有姿态控制测试：

  pytest src/xr_rm_teleop/test/test_orientation_control.py
  

• 从工作空间根目录构建：

  source /opt/ros/humble/setup.bash
  colcon build --symlink-install
  

11.2 左右臂单独 Mock 验收

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