acRealman_xr/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md

# RM75 SO(3) 姿态跟随与 OmniPicker 模型 Implementation Plan

> **For Codex:** REQUIRED SUB-SKILL: Use `superpowers:executing-plans` to implement this plan task-by-task.

**Goal:** 去掉遥操作控制路径中的 RPY 往返转换，使 RM75 TCP 姿态始终沿 SO(3) 最短路径跟随，并让左右臂的 Placo QP 直接控制一体化模型中的 `omnipicker_tcp`。

**Architecture:** 保留现有单节点、单步 Placo QP、关节反馈、RealMan 连接和安全停止链路。XR 四元数映射为机器人旋转矩阵；平移使用直接位置差，姿态使用 SO(3) 对数误差，二者以解耦 `3+3` 形式处理。Placo 接收完整 `4×4` 目标矩阵并直接约束 URDF 的 `omnipicker_tcp`，不再读取外设工具位姿做 QP 末端换算。

**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、NumPy、Placo 0.9.4、Pinocchio 3.7.0、pytest、URDF。

**Repository rule:** 不执行 `git commit`、`git push` 或真机命令。所有启动验证必须显式使用 `use_mock:=true`；`peripherals_rm75.yaml`、`avoid_singularity`、可操作度任务和既有安全限制保持不变。

---

## 文件范围

- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
- Modify: `xr_rm_teleop/test/test_joint_control.py`
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
- Modify: `README.md`

不删除旧 `xr_rm_teleop/models/rm75` 资源，只让 launch 停止选用它，避免扩大无关清理范围。

### Task 1: 导入 fixed 一体化模型并定义 TCP

**Files:**

- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`

- [x] **Step 1: 先写模型结构失败测试**

在 `test_placo_transforms.py` 中用 `xml.etree.ElementTree` 读取 fixed URDF，断言：

```python
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
assert tcp_joint.attrib["type"] == "fixed"
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
```

运行：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_placo_transforms.py
```

Expected: FAIL，模型包尚不存在。

- [x] **Step 2: 从上传 ZIP 只导入运行所需资源**

从
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
导入 fixed URDF 和两组 mesh 到
`xr_rm_teleop/models/rm75_omnipicker`；不导入独立描述包元数据、示例脚本、
活动式 URDF 或额外验证文档。保留上传模型的几何、惯量、关节限制和 fixed
OmniPicker 关节，并在 `xr_rm_teleop/setup.py` 中安装这些资源。

- [x] **Step 3: 在 fixed URDF 增加已确认的 TCP**

```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>
```

- [x] **Step 4: 重跑模型测试**

Expected: PASS；运动关节仍严格为 `joint_1` 至 `joint_7`，TCP 偏移为
`+Z 0.16 m`。

### Task 2: 让 Placo 直接接收 SE(3) 并约束 `omnipicker_tcp`

**Files:**

- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`

- [x] **Step 1: 先把变换和 smoke 测试改为矩阵接口**

测试改为：

```python
current = solver.update_joint_state(joints)
assert current.shape == (4, 4)
target = current.copy()
target[0, 3] += 0.01
target[:3, :3] = rotation_delta @ target[:3, :3]
joints = solver.solve(target)
```

同时覆盖非法形状、NaN 和非 SE(3) 最后一行会被拒绝。smoke 使用
`dt=1/125`，以旋转矩阵相对角度计算姿态误差，不再转换 RPY。

运行现有两项测试，确认它们先因旧 `ArmPose/tool_pose` 接口失败。

- [x] **Step 2: 最小化求解器接口**

将构造函数改为：

```python
PlacoIkSolver(urdf_path: str, dt: float)
```

并完成以下替换：

- 删除 `_rpy_to_rotation`、`_rotation_to_rpy`、`_arm_pose_to_transform`、
  `_transform_to_arm_pose`、`_tool_pose_to_transform`。
- 删除 `_tool_transform` 和 `_tool_inverse`。
- frame task 从 `link_7` 改为 `omnipicker_tcp`。
- 可操作度任务继续作用于原来的 `link_7`，并保留原权重
  `soft, 5e-2`。
- `update_joint_state()` 直接返回
  `get_T_world_frame("omnipicker_tcp").copy()`。
- `solve()` 校验并直接设置传入的 `4×4` 目标矩阵。
- frame task、动能正则、虚拟基座固定、关节位置/速度校验保持原状。

- [x] **Step 3: 运行纯单元测试**

```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_placo_transforms.py
```

Expected: PASS。该命令不构造 Placo，不要求系统 Python 安装厂商 SDK。

### Task 3: 用 SO(3) 最短路径替换 RPY 姿态控制

**Files:**

- Modify: `xr_rm_teleop/test/test_orientation_control.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`

- [x] **Step 1: 先写 SO(3) 回归测试**

保留零四元数停止测试，并增加以下最小覆盖：

- `q` 与 `-q` 得到同一旋转矩阵。
- 初始 pitch 接近 `+90°`、`-90°` 时，小手柄旋转只产生同量级的小旋转。
- 跨过旧 RPY 分支时，相对旋转仍取最短路径。
- 死区按 `norm(Log(R_target R_currentᵀ))` 判断。
- `alpha=0.5` 时 SO(3) 误差角减半。
- `dt=1/125`、`max_orientation_speed=0.5` 时单步不超过 `0.004 rad`。
- 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
- 矩阵转调试四元数后有限且单位化。

运行：

```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_orientation_control.py
```

Expected: FAIL，旧代码仍返回和处理 RPY。

- [x] **Step 2: 实现最少的 NumPy SO(3) 运算**

在现有遥操作模块中加入并只加入实际调用的函数：

```text
quaternion -> rotation matrix
rotation matrix -> normalized quaternion
Log_SO3(rotation) -> 3D rotation vector
Exp_SO3(rotation vector) -> rotation matrix
position + rotation -> 4×4 transform
```

输入必须有限。近似旋转矩阵仅在
`norm(RᵀR-I) <= 1e-3` 且行列式为正时用 SVD 投影；明显无效输入抛出
`ValueError`。`Log_SO3` 在接近 `π` 时仍返回最短的有限旋转向量。

- [x] **Step 3: 替换姿态目标、滤波和限速**

控制路径统一为：

```python
R_xr_delta = R_xr_now @ R_xr_start.T
R_robot_delta = mapping @ R_xr_delta @ mapping.T
axis_delta = log_so3(R_robot_delta)
axis_delta[disabled_axes] = 0.0
R_raw = exp_so3(axis_delta) @ R_robot_start

error = log_so3(R_target @ R_current.T)
R_next = exp_so3(scale * error) @ R_current
```

继续分别保存平移列表和旋转矩阵状态，但构造 QP 目标与调试目标时合成为
`4×4` 矩阵。删除控制路径中的 `_matrix_to_euler`、
`_quaternion_to_euler`、分量 `_angle_delta` 及 RPY
死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。

- [x] **Step 4: 重跑姿态测试**

Expected: PASS。

### Task 4: 把节点状态、QP 和调试话题贯通为 SE(3)

**Files:**

- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `xr_rm_teleop/test/test_joint_control.py`

- [x] **Step 1: 先将关节控制测试改为 `4×4` 矩阵**

Fake solver 的 `update_joint_state()` 返回有限齐次矩阵；QP
成功、失败和首帧反馈测试均断言矩阵接口。运行测试，确认旧类型假设失败。

- [x] **Step 2: 完成节点矩阵状态迁移**

- `_robot_start_pose`、`_last_current_pose` 和调试 fallback 改存 `4×4`
  矩阵。
- `PlacoIkSolver` 初始化不再接收
  `self._peripheral_config.tool_pose`；外设配置仍只传给
  `RealManAdapter.configure_peripheral()`。
- 原始目标与发送目标均合成为 `omnipicker_tcp` 的 SE(3)。
- `TwistStamped.angular` 使用
  `Log(R_sent R_previousᵀ) / dt`，表达在 `rm_base`。
- `PoseStamped` 只在发布边界把旋转矩阵转四元数。
- QP 异常继续返回 last-known-good；Grip 松开、超时、反馈错误和发送错误继续
  走现有慢停与状态重置。

- [x] **Step 3: 运行相关单元测试**

```bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_orientation_control.py \
  src/xr_rm_teleop/test/test_joint_control.py \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  src/xr_rm_teleop/test/test_initial_joint_pose.py
```

Expected: PASS。

### Task 5: 切换 launch 模型并同步已确认参数

**Files:**

- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
- Modify: `xr_rm_teleop/setup.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
- Modify: `README.md`

- [x] **Step 1: 修改模型来源**

`_rm75_urdf()` 改为：

```python
PathJoinSubstitution([
    FindPackageShare("xr_rm_teleop"),
    "models",
    "rm75_omnipicker",
    "urdf",
    "RM75-B_OmniPicker_fixed.urdf",
])
```

并让 `xr_rm_teleop/setup.py` 安装该目录下的 fixed URDF 和两组 mesh。

- [x] **Step 2: 只修改已确认参数**

节点默认值、launch 默认值和三份 YAML 对应项同步：

```yaml
control_rate_hz: 125.0
orientation_deadband_rad: 0.005
orientation_filter_alpha: 0.65
max_orientation_speed: 0.5
follow: false
```

其中右臂 YAML 的
`move_to_initial_pose_on_connect: True`
改为 `false`。不修改任何工作空间、圆柱、线速度、关节速度、初始角、
`avoid_singularity`、安全配置或外设配置。

- [x] **Step 3: 更新 README 中已失真的运行说明**

只更新：

- 默认控制频率 `90.0 -> 125.0`。
- QP 模型改为一体化 fixed URDF，并直接控制 `omnipicker_tcp`。
- 姿态死区、滤波和限速使用 SO(3) 最短路径，不使用 RPY。
- `peripherals_rm75.yaml` 仍只用于真实控制器工具坐标、负载和外设选择，不再
  参与 Placo TCP 矩阵换算。

### Task 6: 构建、数值 smoke 与 mock 启动验证

**Files:**

- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`

- [x] **Step 1: 构建整个工作空间**

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```

Expected: `xr_rm_teleop` 和 `xr_rm_bringup` 构建成功。

- [x] **Step 2: 运行指定姿态测试和相关回归测试**

```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_orientation_control.py

PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_joint_control.py \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  src/xr_rm_teleop/test/test_initial_joint_pose.py
```

Expected: PASS。

- [x] **Step 3: 使用固定 XR Python 运行 Placo 数值 smoke**

```bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/placo_ik_smoke.py \
  install/xr_rm_teleop/share/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf
```

对左右初始关节姿态分别验证：

- 七个运动关节及顺序正确。
- `omnipicker_tcp` 相对 `link_7` 为 `[0, 0, 0.16]`、单位旋转。
- QP 输出七个有限关节角并满足位置与单周期速度限制。
- 目标停止两秒时打印最大关节变化，但不把漂移设为失败条件。
- 运动目标最终 TCP 位置误差 `<= 5 mm`，姿态误差 `<= 2°`。
- 打印平均/最大求解耗时及超过 `8 ms` 周期预算的次数，只记录、不设机器相关
  的硬失败阈值。

- [x] **Step 4: 只启动 mock**

分别短时启动：

```bash
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
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```

确认 fixed URDF、Placo 和左右节点名加载成功，无 RealMan SDK 导入或网络连接。
由人工结束 mock launch；Codex 不执行任何 `use_mock:=false` 命令。

- [x] **Step 5: 最终范围检查**

```bash
git diff --check
git status --short
git diff -- \
  src/xr_rm_teleop \
  src/xr_rm_bringup \
  src/README.md \
  src/docs/superpowers
```

确认 `peripherals_rm75.yaml`、`avoid_singularity`、可操作度权重和所有既有安全
限制未被改变。