# 双 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`。