YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
687a0b401a7698364395539ac356ae55b1435b3d
acRealman_xr/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md
T

13 KiB
Raw Blame History

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

  • Step 1: 先写模型结构失败测试

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

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"

运行：

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，模型包尚不存在。

  • 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 中安装这些资源。

  • Step 3: 在 fixed URDF 增加已确认的 TCP
<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>
  • 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

  • Step 1: 先把变换和 smoke 测试改为矩阵接口

测试改为：

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 接口失败。

  • Step 2: 最小化求解器接口

将构造函数改为：

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、动能正则、虚拟基座固定、关节位置/速度校验保持原状。

  • Step 3: 运行纯单元测试

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

  • 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。
  • 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
  • 矩阵转调试四元数后有限且单位化。

运行：

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

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

  • Step 2: 实现最少的 NumPy SO(3) 运算

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

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 在接近 π 时仍返回最短的有限旋转向量。

  • Step 3: 替换姿态目标、滤波和限速

控制路径统一为：

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 死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。

  • 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

  • Step 1: 先将关节控制测试改为 4×4 矩阵

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

  • 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 松开、超时、反馈错误和发送错误继续 走现有慢停与状态重置。

  • Step 3: 运行相关单元测试

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

  • Step 1: 修改模型来源

_rm75_urdf() 改为：

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

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

  • Step 2: 只修改已确认参数

节点默认值、launch 默认值和三份 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、安全配置或外设配置。

  • 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

  • Step 1: 构建整个工作空间

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

Expected: xr_rm_teleop 和 xr_rm_bringup 构建成功。

  • Step 2: 运行指定姿态测试和相关回归测试
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。

  • Step 3: 使用固定 XR Python 运行 Placo 数值 smoke
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 周期预算的次数，只记录、不设机器相关 的硬失败阈值。

  • Step 4: 只启动 mock

分别短时启动：

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 命令。

  • Step 5: 最终范围检查
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、可操作度权重和所有既有安全 限制未被改变。

Reference in New Issue View Git Blame Copy Permalink