# O30 右手三相机标定与 URDF 修正操作说明

> 支持范围：O30 右手、20 个有效电机、20 个主动 URDF 关节。

## 1. 固定输入

O30 SDK 工程：

```text
/home/lxp/projects/linkerhand-o30-ros2
```

原始 CAD URDF：

```text
/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf
```

标定基准命令固定为：

```text
[0, 0, 255, 205, 165, 20,
 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
```

20 个通道均按完整 `255→0→255` 行程扫描。Tag 0～10 的机位、ID 和
父子连杆角色与 G20 右手标定保持一致。

## 2. SDK 与 URDF 映射

| SDK 下标 | SDK 名称 | URDF 关节 |
|---:|---|---|
| 0 | `thumb_roll` | `thumb_cmc_roll` |
| 1 | `thumb_yaw` | `thumb_cmc_yaw` |
| 2～5 | `index/middle/ring/little_yaw` | 四指 `mcp_roll` |
| 6 | `thumb_root1` | `thumb_mcp` |
| 7～10 | 四指 `root1` | 四指 `mcp_pitch` |
| 11～14 | 四指 `root2` | 四指 `pip` |
| 15 | `thumb_tip` | `thumb_ip` |
| 16～19 | 四指 `tip` | 四指 `dip` |

O30 没有 `thumb_cmc_pitch`，也没有 G20 的被动 IP/DIP；O30 JSON 中的
20 个 URDF 关节全部标记为主动关节。

## 3. 扫描任务

右手以小指为四指参考源，共执行 8 个任务、每项 3 轮：

| 顺序 | 关节 | 电机 | 机位 |
|---:|---|---:|---|
| 1 | `thumb_cmc_roll` | 0 | front |
| 2 | `thumb_mcp` | 6 | front |
| 3 | `thumb_ip` | 15 | front |
| 4 | `pinky_mcp_roll` | 5 | front |
| 5 | `pinky_mcp_pitch` | 10 | side |
| 6 | `pinky_pip` | 14 | side |
| 7 | `pinky_dip` | 19 | side |
| 8 | `thumb_cmc_yaw` | 1 | top |

总计 `8 × 3 × 2 = 48` 个唯一扫描方向。O30 每一轮固定按
`0→255`、`255→0` 完成一次往返，因此单关节三轮均为 `0→255→0`，并在
每轮第一个方向前重新初始化活动机位的PnP跟踪。状态中的计划进度只统计48个
唯一方向，另行显示包含自动重扫在内的实际执行方向次数。扫描小指 `mcp_roll` 时，电机
2/3/4 固定为 255，使未贴 Tag 的三指避开正面机位。

O30 命令增大时 URDF 动态角度增大；这与 G20 的命令—角度方向相反，标定
JSON 会保存单调非递减曲线，并在各电机自己的基准命令处严格归零。

O30 的固件反馈端点允许使用独立于 G20 的到位死区：一般通道为 `±4 u8`；
拇指 CMC 侧摆电机 0 在低速命令 `255` 端为 `±9 u8`；
食指 MCP 侧摆电机 2 在命令 `255` 端为 `±8 u8`；
O30 固件的速度 `0` 仍然很快，且实机位置-时间模式无法可靠连续往返，因此标定
强制使用普通位置模式和最低内部速度 `o30_internal_speed_u8:=0`。标定节点只发送
最终目标，O30 SDK 在独立的约 125 Hz 控制线程中生成单字节位置斜坡，避免相机/PnP
计算延迟导致 3～5 u8 的补偿跳步。默认 `o30_command_full_range_seconds:=6.0`，
完整走过 `0～255` 约需 6 秒，两个方向采用相同限速。暂停、终止或异常时立即把
斜坡目标替换为当前反馈并恢复直控模式。该策略不改变安全扫描范围，也不改变四指
MCP 侧摆静态零位固定为 CAD 0 的规则。拇指 CMC 侧摆使用 `1 u8` 细步，其余
通道累计 `3 u8` 后再向固件更新目标；后者为每个目标留出保持时间，避免电机6等
关节因持续刷新单格目标而一直不启动，总体斜率和正反向用时保持不变。
拇指 MCP（电机 6）在命令 0 端实测稳定反馈为 7，因此该端使用 `±8 u8`。
这些门限只判断固件是否已经稳定到位，不改变实际下发的 `0～255` 扫描范围。

## 4. 静态 URDF 零位策略

- 四指 `mcp_roll`：测量小指动态曲线，但四个关节的静态 URDF 零偏全部固定为 0。
- 小指 `mcp_pitch`：可由下游 PIP 轴观测并通过留出验证后修正。
- 小指 `pip`：可由下游 DIP 轴线相位观测并通过留出验证后修正。
- 小指 `dip`、拇指 `ip`：没有下游观察关节，静态零偏固定为 0。
- 食指、中指、无名指：动态曲线从小指曲线按各自基准命令重新归零；静态偏差不继承。
- 拇指 `cmc_roll/cmc_yaw/mcp`：按相邻下游轴逐级求解并执行第三轮留出验证。

修正 URDF 只改通过验证的关节 `origin.rpy`，不改 `origin.xyz`、`axis`、
mesh、连杆长度和 CAD 限位。生成后的 URDF 不能作为下一次标定的源文件。

## 5. 启动

先保证 O30 SDK 已编译，并依次加载 ROS、O30 SDK 和本工程：

```bash
source /opt/ros/jazzy/setup.bash
source /home/lxp/projects/linkerhand-o30-ros2/install/setup.bash
source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
```

预检（不运动）：

```bash
ros2 launch g20_thumb_apriltag_calibration \
  three_camera_calibration.launch.py \
  hand_model:=O30 hand_type:=right \
  serial_number:=O30_RIGHT_001 \
  source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
  start_sdk:=false commands_enabled:=false
```

正式启动，蓝色/黑色厂商 CANFD 盒：

```bash
ros2 launch g20_thumb_apriltag_calibration \
  three_camera_calibration.launch.py \
  hand_model:=O30 hand_type:=right \
  serial_number:=O30_RIGHT_001 \
  source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
  o30_comm_type:=libcanbus canfd_device:=0 o30_auto_setup:=false \
  o30_internal_speed_u8:=0 o30_command_full_range_seconds:=6.0
```

三个机位预检通过后查看状态并启动：

```bash
ros2 topic echo /o30_calibration/status_text
ros2 service call /o30_calibration/start std_srvs/srv/Trigger {}
```

暂停、继续和终止服务分别为：

```text
/o30_calibration/pause
/o30_calibration/resume
/o30_calibration/abort
```

## 6. 产物

会话目录中生成：

```text
raw_samples.jsonl
o30_right_<serial>_calibration.json
```

修正 URDF 默认生成在原始 URDF 同目录，文件名包含
`zero_calibrated_<serial>_<timestamp>`。JSON 的动态角度必须和这次生成的修正
URDF 配套使用；运行时不能再次叠加 `urdf_zero_offset_rad`。