# 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 ``` - [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`、可操作度权重和所有既有安全限制未被改变。