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

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) 矩阵：

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 初始旋转矩阵。每周期计算：

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 分量实现。

姿态处理统一使用基坐标系下的左乘增量：

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 形式：

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 中增加：

<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 改为：

构造参数：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 配置首轮保持：

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 对应节点中同步：

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 执行：

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