feat: add RM75 Placo single-step QP inverse kinematics design document
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -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，未升级或全局安装
|
||||
数值依赖。
|
||||
@@ -1,20 +0,0 @@
|
||||
schema: spec-driven
|
||||
|
||||
# Project context (optional)
|
||||
# This is shown to AI when creating artifacts.
|
||||
# Add your tech stack, conventions, style guides, domain knowledge, etc.
|
||||
# Example:
|
||||
# context: |
|
||||
# Tech stack: TypeScript, React, Node.js
|
||||
# We use conventional commits
|
||||
# Domain: e-commerce platform
|
||||
|
||||
# Per-artifact rules (optional)
|
||||
# Add custom rules for specific artifacts.
|
||||
# Example:
|
||||
# rules:
|
||||
# proposal:
|
||||
# - Keep proposals under 500 words
|
||||
# - Always include a "Non-goals" section
|
||||
# tasks:
|
||||
# - Break tasks into chunks of max 2 hours
|
||||
Reference in New Issue
Block a user