242 lines
10 KiB
Markdown
242 lines
10 KiB
Markdown
# 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
|
||
<link name="omnipicker_tcp"/>
|
||
|
||
<joint name="omnipicker_tcp_joint" type="fixed">
|
||
<parent link="omnipicker_base_link"/>
|
||
<child link="omnipicker_tcp"/>
|
||
<origin xyz="0 0 0.16" rpy="0 0 0"/>
|
||
</joint>
|
||
```
|
||
|
||
该变换表示 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. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
|