7.6 KiB
双 RM75 逆解模型替换设计
背景与目标
当前左右遥操作节点都加载单臂 rm75_omnipicker URDF，求解器将 7 个关节名、
q[7:14] 和 omnipicker_tcp 写死。项目新增的
xr_rm_teleop/models/dual_rm75/Dual_arm.urdf 包含真实双臂布局：物理左臂为
scissor 分支，物理右臂为 omnipic 分支，主要活动区域位于机器人前方。
本次变更目标是：
- 单臂和双臂调试都加载同一份
dual_rm75模型； - 左右节点继续独立控制各自的 RM75，只求解当前侧 7 个关节；
- 保留左右臂各自的局部控制坐标系和现有 PICO 映射；
- 使用 URDF 中的 TCP 长度，并同步真机外设工具坐标；
- 把局部后方工作空间余量限制为
0.10 m； - 保留现有速度、工作空间、圆柱、超时和安全停止逻辑。
本次不增加双臂碰撞规避、公共坐标系目标、双臂协同任务，不合并左右控制节点， 也不连接或移动真机。
方案选择
采用“完整双臂 URDF + 两个独立局部相对位姿任务”。
未采用以下方案：
- 公共坐标系绝对位姿任务：需要重写 PICO 映射和现有安全限位，改动范围过大。
- 从双臂模型拆出两份单臂 URDF：会产生重复模型和后续同步风险。
坐标系与控制语义
dual_arm_base_link 是完整模型的公共根坐标系。左右控制节点仍以各自机械臂基座
作为控制和安全坐标系：
| 机械臂 | 局部基坐标系 | TCP | 活动关节 |
|---|---|---|---|
| 左臂 | scissor_base_link |
scissor_scissor_tcp |
scissor_joint_1～scissor_joint_7 |
| 右臂 | omnipic_base_link |
omnipic_OmniPic_tcp |
omnipic_joint_1～omnipic_joint_7 |
以公共坐标系 +X 向机器人右侧、+Y 向前、+Z 向上为参照，URDF 中局部轴
朝向如下：
| 局部轴 | 左臂 scissor_base_link |
右臂 omnipic_base_link |
|---|---|---|
+X |
向下 | 向上 |
+Y |
向后 | 向后 |
+Z |
向左、远离机身 | 向右、远离机身 |
-Y |
向前 | 向前 |
现有左右 xr_to_robot_matrix 继续把 PICO 相对位置和相对旋转映射到对应局部基
坐标系。节点产生的目标仍是 T_base_tcp，不显式转换成
dual_arm_base_link 下的绝对目标。
求解器设计
左右节点使用同一个 PlacoIkSolver 类，但每个节点创建自己的求解器实例、机器人
状态和 QP 任务。两个实例都加载完整 Dual_arm.urdf，不共享可变状态。
求解器构造时接收 arm=left|right，按固定映射选择局部基坐标系、TCP、当前侧
关节和另一侧关节。每个实例执行以下设置：
- 使用
mask_fbase(True)固定 Placo 浮动基座； - mask 另一侧全部 7 个关节；
- 使用 Placo 原生
add_relative_frame_task(base_frame, tcp_frame, target)创建局部 TCP 任务； - 保留速度限制、动能正则化、最多 30 次有界迭代和现有收敛阈值。
双臂模型的 Placo 状态为 21 个 q 分量：7 个浮动基座分量、右臂 7 个关节、
左臂 7 个关节。求解器不再使用固定 q[7:14]，而是通过当前侧关节名查询：
get_joint_offset()：定位实际关节反馈和逆解结果在 q 中的位置；get_joint_v_offset()：定位对应的 URDF 关节速度上限。
当前 URDF 中右臂 q/v offset 分别为 7～13/6～12，左臂分别为
14～20/13～19；实现仍通过名称查询并对这些预期结果做回归测试。
查询 offset 不放宽模型校验。求解器仍检查完整左右关节集合、当前侧恰好 7 个关节、 offset 唯一有效，以及所需 base 和 TCP 均存在。
update_joint_state() 只写入当前侧 7 个关节反馈，并返回当前 TCP 相对当前侧基座的
T_base_tcp。solve() 接受相同坐标语义的目标，设置相对位姿任务并只返回当前侧
7 个关节结果。另一侧关节保持 mask，不参与本实例求解。
启动、安装与配置
arm_debug.launch.py 的 arm:=left|right|both 全部使用：
xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
双臂模式继续保留 left_arm_teleop、right_arm_teleop 节点名，use_mock 默认
保持 true。setup.py 安装 Dual_arm.urdf 以及 dual_rm75/meshes 中现有的
.STL 和 .stl 文件，不新增依赖。
三份控制配置的局部工作空间统一为：
workspace_min: [-0.70, -0.70, 0.10]
workspace_max: [0.70, 0.10, 0.75]
其中两侧局部 -Y 都是机器人前方，+Y 后方最多保留 0.10 m 余量。其他工作
空间轴、圆柱限位、线速度、角速度、关节速度、关节加速度和指令超时参数不变。
真机外设配置采用 URDF TCP 长度，但保留当前硬件选择编号：
tools_in_ee:
scissor:
pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
omnipic:
pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
minisci:
pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
arms:
left:
scissorgripper: 2
right:
scissorgripper: 1
左臂保留编号 2，继续使用控制器 DO3/DO4；该编号按当前配置顺序选中
minisci 工具坐标，因此更新 minisci.pose.z。右臂编号 1 继续选中
omnipic。URDF 的左分支名 scissor_* 与真机外设编号/配置键是两套既有命名，
不据此改写硬件编号。两侧负载参数和未选中 scissor.pose 保持不变。
README 同步说明新模型路径、左右分支/TCP、局部坐标轴和前方工作区。
校验与故障处理
模型路径、arm、关节、frame 或 offset 校验失败时，节点在创建 RealMan 适配器前 终止启动，不连接真机。
运行期间保留现有行为：
- 关节反馈必须包含 7 个有限数值；
- TCP 目标必须是有限、合法的齐次变换和旋转矩阵；
- QP 结果必须满足当前侧 URDF 关节位置和单周期速度限制；
- QP 不收敛时保持上一组有效关节目标；
- 反馈异常、反馈超时、XR 超时、Grip 松开和节点退出时执行现有安全停止；
configure_safety_limits保持true；move_to_initial_pose_on_connect默认保持false。
完整 URDF 虽包含两臂碰撞几何，本次不启用碰撞约束。真机验证不在本次执行范围； 后续首次真机验证必须分别验证两臂并保持物理隔离。
测试与验收
采用现有 pytest、Placo 0.9.4 和 ROS2 构建流程，不新增测试框架。
自动化测试覆盖：
- 双臂 URDF 的 14 个活动关节、base、TCP、固定挂载和 TCP 长度；
- 左右实例选择正确的关节、q/v offset 和相对任务 frame；
- 当前实例只更新和返回本侧 7 个关节，另一侧保持不动；
- 左右初始关节反馈能得到有限的局部
T_base_tcp； - 左右小幅可达目标能够收敛，结果满足位置、姿态和关节限制；
- 非法目标、未初始化求解和不收敛故障路径；
- 左臂编号
2实际选择minisci且 TCP 为0.165 m； - 右臂编号
1实际选择omnipic且 TCP 为0.14 m； - 三份配置的局部 Y 上界均为
0.10 m。
所有命令在 /home/robot/WS_xr 执行，并先加载 ROS2 Humble：
source /opt/ros/humble/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
src/xr_rm_teleop/test/test_placo_transforms.py -v
pytest src/xr_rm_teleop/test/test_orientation_control.py
colcon build --symlink-install
source install/setup.bash
timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
arm:=both use_mock:=true
最后一条命令只验证安装空间中的新模型能被两个 mock 节点加载；timeout 到期退出
属于预期。整个验收过程不得使用 use_mock:=false。