# 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. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。