181 lines
7.6 KiB
Markdown
181 lines
7.6 KiB
Markdown
# 双 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 + 两个独立局部相对位姿任务”。
|
||
|
||
未采用以下方案：
|
||
|
||
1. 公共坐标系绝对位姿任务：需要重写 PICO 映射和现有安全限位，改动范围过大。
|
||
2. 从双臂模型拆出两份单臂 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、当前侧
|
||
关节和另一侧关节。每个实例执行以下设置：
|
||
|
||
1. 使用 `mask_fbase(True)` 固定 Placo 浮动基座；
|
||
2. mask 另一侧全部 7 个关节；
|
||
3. 使用 Placo 原生
|
||
`add_relative_frame_task(base_frame, tcp_frame, target)` 创建局部 TCP 任务；
|
||
4. 保留速度限制、动能正则化、最多 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` 全部使用：
|
||
|
||
```text
|
||
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` 文件，不新增依赖。
|
||
|
||
三份控制配置的局部工作空间统一为：
|
||
|
||
```yaml
|
||
workspace_min: [-0.70, -0.70, 0.10]
|
||
workspace_max: [0.70, 0.10, 0.75]
|
||
```
|
||
|
||
其中两侧局部 `-Y` 都是机器人前方，`+Y` 后方最多保留 `0.10 m` 余量。其他工作
|
||
空间轴、圆柱限位、线速度、角速度、关节速度、关节加速度和指令超时参数不变。
|
||
|
||
真机外设配置采用 URDF TCP 长度，但保留当前硬件选择编号：
|
||
|
||
```yaml
|
||
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：
|
||
|
||
```bash
|
||
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`。
|