docs: 添加双 RM75 逆解设计与计划
This commit is contained in:
@@ -0,0 +1,180 @@
|
||||
# 双 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`。
|
||||
Reference in New Issue
Block a user