670 lines
24 KiB
Markdown
670 lines
24 KiB
Markdown
# G20 三相机零位标定与 URDF 修正操作说明
|
||
|
||
> 适用工程：`linkerhand_retarget_ros2`
|
||
> 适用标定包：`g20_thumb_apriltag_calibration`
|
||
> 文档基线：2026-08-11 当前 V3 轴坐标系逻辑
|
||
> 适用对象：G20 左手和右手
|
||
|
||
## 1. 文档目的
|
||
|
||
本文说明当前代码中 G20 三相机、11 个 AprilTag 的完整标定逻辑及现场操作流程，包含：
|
||
|
||
- 20 通道 u8 命令到 21 个 URDF 关节动态角度曲线的标定；
|
||
- 拇指 CMC roll/yaw/pitch 的静态 URDF 零位求解；
|
||
- 四指动态曲线继承和静态 CAD 零位保护；
|
||
- 第三轮留出验证、自动重采、暂停和恢复策略；
|
||
- 从原始 CAD URDF 生成修正 URDF；
|
||
- 使用已有 `raw_samples.jsonl` 进行无运动离线重放；
|
||
- 修正 URDF 和配套 JSON 的仿真使用方法。
|
||
|
||
本文中的右手 `G20_RIGHT_001/20260811_120146` 数值仅用于说明当前算法的实际结果，**不是代码中写死的标定角度，也不是其他机械手的目标值**。
|
||
|
||
## 2. 标定的最终产物
|
||
|
||
一次完整标定同时生成两类互相配套的产物：
|
||
|
||
1. 精简标定 JSON：保存每个关节按命令 `0～255` 索引的 256 点 `angle_rad` 动态曲线，以及 URDF 静态零偏和质量指标。
|
||
2. 修正 URDF：只把通过验证的静态零偏写入原始 CAD URDF 的主动关节 `origin.rpy`。
|
||
|
||
运行时必须遵守以下规则：
|
||
|
||
- 修正 URDF 已经包含静态 `urdf_zero_offset_rad`，运行时不能再加一次；
|
||
- 动态关节位置必须使用同一机械手、同一侧、同一次标定 JSON 中的 `angle_rad`；
|
||
- 左手曲线不能用于右手，右手曲线不能用于左手；
|
||
- 不能用已经修正过的 URDF 作为下一次标定的源文件。
|
||
|
||
## 3. 硬件和坐标配置
|
||
|
||
### 3.1 三台相机
|
||
|
||
默认机位和序列号如下：
|
||
|
||
| 机位 | 默认序列号 | 默认作用 |
|
||
|---|---|---|
|
||
| 正面 `front` | `DB2163742` | 拇指 CMC pitch/roll、MCP/IP、参考指 MCP roll |
|
||
| 侧面 `side` | `DB2163749` | 参考指 MCP pitch、PIP/DIP |
|
||
| 上面 `top` | `DB2163739` | 拇指 CMC yaw |
|
||
|
||
默认采集参数：
|
||
|
||
- 分辨率和像素格式由海康节点配置；
|
||
- 帧率：`30 Hz`；
|
||
- 曝光：`5000 us`；
|
||
- 增益：`0 dB`；
|
||
- 自动曝光：关闭；
|
||
- AprilTag 检测降采样：`decimate=1.5`；
|
||
- 启动文件固定使用 `rmw_fastrtps_cpp` 和 64 MB Fast DDS 共享内存配置。
|
||
|
||
每台相机必须有对应当前镜头、焦距、分辨率的独立内参文件：
|
||
|
||
```text
|
||
~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||
~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||
~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||
```
|
||
|
||
三相机外参默认文件：
|
||
|
||
```text
|
||
/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml
|
||
```
|
||
|
||
移动相机、改变镜头焦距/对焦、改变分辨率或重新标定内参后，必须重新标定外参。
|
||
|
||
### 3.2 11 个 AprilTag
|
||
|
||
使用 `tag36h11`，有效黑框边长配置为 `16 mm`，不包含外围白边。
|
||
|
||
| Tag ID | 机位 | 固定位置或运动件 |
|
||
|---:|---|---|
|
||
| 0 | 正面 | 掌壳固定基准 |
|
||
| 1 | 正面 | 拇指 CMC 后连杆 |
|
||
| 2 | 正面 | 拇指 MCP 后连杆 |
|
||
| 3 | 正面 | 拇指 IP 后末节 |
|
||
| 4 | 侧面 | 掌壳侧面固定基准 |
|
||
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
|
||
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
|
||
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
|
||
| 8 | 上面 | 掌壳或底座固定基准 |
|
||
| 9 | 上面 | 拇指 CMC yaw 运动件 |
|
||
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
|
||
|
||
Tag 必须固定在刚性件上，不能跨关节、贴在软胶上、在运动中翘起或移动。Tag 的平面内旋转不要求贴正，但整个标定会话中安装姿态必须保持不变。
|
||
|
||
## 4. 左右手配置差异
|
||
|
||
左右手使用同一套拟合、留出验证和 URDF 写入算法，但使用独立的参考指、电机和镜像避挡姿态。
|
||
|
||
| 项目 | 左手 | 右手 |
|
||
|---|---|---|
|
||
| 四指参考源 | 食指 `index` | 小指 `pinky` |
|
||
| 扫描电机顺序 | `0/5/15/6/1/16/10` | `0/5/15/9/4/19/10` |
|
||
| 参考指 MCP roll | 电机 6 | 电机 9 |
|
||
| 参考指 MCP pitch | 电机 1 | 电机 4 |
|
||
| 参考指 PIP | 电机 16 | 电机 19 |
|
||
| 参考指 roll 避挡 | 其他侧摆电机置 0 | 其他侧摆电机置 255 |
|
||
| 电机 0 辅助姿态 | 使用基准姿态 | 电机 5、10 固定 255 |
|
||
|
||
两侧的非零拇指静态零偏都必须由各自当前会话的数据计算，不共享任何标定角度。
|
||
|
||
## 5. 扫描任务和运动策略
|
||
|
||
### 5.1 固定基准命令
|
||
|
||
调用 `/g20_calibration/start` 后，程序先下发并确认以下 20 通道姿态：
|
||
|
||
```text
|
||
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||
```
|
||
|
||
普通关节以命令 `255` 为动态曲线零点；四指 MCP 侧摆以命令 `127` 为动态曲线零点。
|
||
|
||
### 5.2 七个扫描任务
|
||
|
||
每个任务执行 3 轮 `255→0→255`，即每个任务 6 个方向。总计：
|
||
|
||
```text
|
||
7 个任务 × 3 轮 × 2 个方向 = 42 个扫描方向
|
||
```
|
||
|
||
| 顺序 | 任务 | 电机 | 机位 | 同时拟合 |
|
||
|---:|---|---:|---|---|
|
||
| 1 | 拇指 CMC pitch | 0 | 正面 | `thumb_cmc_pitch` |
|
||
| 2 | 拇指 CMC roll | 5 | 正面 | `thumb_cmc_roll` |
|
||
| 3 | 拇指 MCP | 15 | 正面 | `thumb_mcp`、被动 `thumb_ip` |
|
||
| 4 | 参考指 MCP roll | 左6/右9 | 正面 | 参考指 `mcp_roll` |
|
||
| 5 | 参考指 MCP pitch | 左1/右4 | 侧面 | 参考指 `mcp_pitch` |
|
||
| 6 | 参考指 PIP | 左16/右19 | 侧面 | 参考指 `pip`、被动 `dip` |
|
||
| 7 | 拇指 CMC yaw | 10 | 上面 | `thumb_cmc_yaw` |
|
||
|
||
特殊辅助姿态：
|
||
|
||
- 扫描拇指 yaw 时，电机 5 固定为 `145`，避免 Tag 9 姿态过斜；
|
||
- 右手扫描电机 0 时，电机 5 和 10 固定为 `255`；
|
||
- 扫描参考指 MCP roll 时，其余三指侧摆移到对应左右手避挡端；
|
||
- 当前方向重试和人工恢复时，辅助姿态保持一致。
|
||
|
||
### 5.3 标定速度
|
||
|
||
G20 SDK 使用五指速度数组：
|
||
|
||
- 常规速度：`15`；
|
||
- 参考指 MCP roll：参考指速度 `5`；
|
||
- 参考指 MCP pitch/PIP：参考指速度 `10`；
|
||
- 自动重试速度比例：`80% / 60% / 50%`，最低速度不低于 `3`。
|
||
|
||
## 6. 轨迹和关节轴拟合
|
||
|
||
### 6.1 时间戳配对
|
||
|
||
每帧 Tag 图像与 20 通道机械手状态按时间戳配对，默认最大允许偏差为 `50 ms`。标定曲线按实际命令分箱，但已确认的固件端点饱和反馈可以归入对应的命令端点分箱。
|
||
|
||
每个扫描方向至少需要：
|
||
|
||
- 40 帧同步有效数据；
|
||
- 覆盖至少 240 个 u8；
|
||
- 至少 32 个有效整数分箱；
|
||
- 相邻有效分箱最大间隔不超过 16；
|
||
- 同时包含命令 0 和 255 端点。
|
||
|
||
### 6.2 动态角度曲线
|
||
|
||
程序使用父/子 Tag 的完整相对旋转轨迹拟合关节转角，分别拟合下降和上升方向，检查单调修正、正反程回差和三轮行程一致性，再生成按命令索引的 256 点 `angle_rad`。
|
||
|
||
四指策略：
|
||
|
||
- 左手实测食指动态曲线，继承给中指、无名指、小指；
|
||
- 右手实测小指动态曲线，继承给食指、中指、无名指；
|
||
- 继承仅用于动态命令—角度关系；
|
||
- 不把参考指的静态装配偏差复制给其他独立电机。
|
||
|
||
### 6.3 三维轴方向和轴线位置
|
||
|
||
当前算法不直接用单帧平面 Tag PnP 姿态作为关节角：
|
||
|
||
- 轴方向主要来自整段相对旋转的螺旋轴；
|
||
- 斜视且三维运动平面可观的关节，保留旋转轴和中心圆轴的交叉检查；
|
||
- 接近端视的关节使用相对姿态轴约束圆轨迹方向；
|
||
- 轴线上一点由整段相对 SE(3) 的 `(I-R)p=t` 方程拟合；
|
||
- 接近沿轴观察时，丢弃单目无法稳定确定的光轴深度，只使用图像平面内可观分量。
|
||
|
||
### 6.4 IPPE 双分支处理
|
||
|
||
每轮运动前在静止端点联合 8 帧选择整组最稳定的平面 Tag PnP 分支。侧面 Tag 4/5/6/7 还检查贴面法向一致性，避免选择低重投影误差但几何镜像的分支。
|
||
|
||
## 7. 当前 V3 静态零位求解逻辑
|
||
|
||
### 7.1 为什么不能用两条根轴线间距确定掌部旋转
|
||
|
||
拇指 CMC roll 根轴和四指 MCP roll 根轴在 CAD 中近似平行。旧逻辑使用两条三维轴线的空间间距确定掌坐标系绕根轴的旋转，但单目 PnP 的固定深度偏差会改变这条间距方向，并被误算成稳定的拇指 roll 静态零偏。
|
||
|
||
这种误差可以三轮高度重复，因此“重复性好”并不能证明绝对零位正确。
|
||
|
||
### 7.2 V3 掌坐标系锚定
|
||
|
||
当前 V3 使用：
|
||
|
||
1. 行程更充分的根轴实测方向；
|
||
2. 保持原始 CAD 直立的参考指 MCP pitch 实测轴方向；
|
||
3. 两条根轴线位置只用于平移，不参与绕根轴旋转。
|
||
|
||
左手使用食指 MCP pitch，右手使用小指 MCP pitch。该逻辑全部由当前会话轨迹计算，不包含按左右手或序列号写死的拇指角度。
|
||
|
||
### 7.3 拇指零位依赖链
|
||
|
||
拇指静态零位按可观测链逐级求解：
|
||
|
||
- `thumb_cmc_yaw` 实测轴方向观测 `thumb_cmc_roll` 零位；
|
||
- `thumb_cmc_pitch` 实测轴方向观测 `thumb_cmc_yaw` 零位；
|
||
- `thumb_mcp` 实测轴线相位观测 `thumb_cmc_pitch` 零位；
|
||
- 被动 `thumb_ip` 的浅圆弧只作为诊断，不能覆盖 `thumb_mcp` 的原始 CAD 静态零位。
|
||
|
||
逐关节一维鲁棒求解可防止远端异常把已经确定的上游零位一起拖到边界。
|
||
|
||
### 7.4 四指静态零位保护
|
||
|
||
当前 11-Tag 布局只能直接观测一根参考指，不能证明四根独立电机具有相同绝对装配相位。因此：
|
||
|
||
- 四指 MCP roll 静态修正固定为原始 CAD 0；
|
||
- 四指 MCP pitch 静态修正固定为原始 CAD 0；
|
||
- 四指 PIP 静态修正固定为原始 CAD 0；
|
||
- `thumb_mcp` 静态修正固定为原始 CAD 0；
|
||
- 这些关节的动态 256 点曲线仍然实测或继承。
|
||
|
||
这里的“0”表示不修改原始 CAD `origin.rpy`，不是额外写入某台机械手的人工标定角度。
|
||
|
||
## 8. 质量门限和留出验证
|
||
|
||
### 8.1 预检门限
|
||
|
||
| 指标 | 默认要求 |
|
||
|---|---:|
|
||
| 预检窗口 | 60 帧 |
|
||
| 所需 Tag 同时有效率 | ≥95% |
|
||
| 检测频率 | ≥15 Hz，正常应接近30 Hz |
|
||
| Hamming | 0 |
|
||
| Decision margin | ≥30 |
|
||
| Tag 最小边长 | ≥30 px |
|
||
| PnP 重投影 RMS | ≤1.5 px |
|
||
| 图像—状态时间差 | ≤50 ms |
|
||
|
||
### 8.2 轨迹和轴门限
|
||
|
||
| 指标 | 默认要求 |
|
||
|---|---:|
|
||
| 主动关节旋转轴外 RMS | ≤2.5° |
|
||
| 被动关节旋转轴外 RMS | ≤7.5° |
|
||
| 三轮轴方向极差 | ≤0.75° |
|
||
| 轴线径向 RMS | ≤3 mm |
|
||
| SE(3) 轴线拟合 RMS | ≤1 mm |
|
||
| 可观三维圆的姿态轴/圆轴夹角 | ≤1° |
|
||
| 父子轴锥角几何不一致 | ≤5° |
|
||
| 主动曲线三轮行程差 | ≤3° |
|
||
| 被动曲线三轮行程差 | ≤10° |
|
||
| 主动最大单调修正 | ≤2° |
|
||
| 主动最大回差 | ≤5° |
|
||
| 被动最大单调修正 | ≤3° |
|
||
| 被动最大回差 | ≤7.5° |
|
||
|
||
拇指可求解静态偏移默认安全范围为 `±20°`。四指静态偏移不由相机相位覆盖，保持原始 CAD。
|
||
|
||
### 8.3 第三轮强制留出
|
||
|
||
前两轮用于训练，第三轮必须作为独立留出验证：
|
||
|
||
- 轨迹角度 MAE ≤1°；
|
||
- 轨迹角度 P95 ≤2°；
|
||
- 非零静态修正必须在第三轮优于原始 URDF；
|
||
- 改善必须通过按三轮分组的 95% bootstrap 置信检查；
|
||
- 最终再使用三轮全部数据重拟合正式结果。
|
||
|
||
`validation_enabled:=false` 只关闭额外随机机械动作，不能关闭第三轮留出验证。
|
||
|
||
## 9. 运动安全和端点处理
|
||
|
||
默认端点容差为 `±2 u8`，只有经过实机确认的固件饱和端点使用专用容差：
|
||
|
||
| 端点 | 专用容差 |
|
||
|---|---:|
|
||
| 拇指 yaw 电机10，命令0 | ±4 u8 |
|
||
| 右手拇指 yaw 电机10，命令255 | ±5 u8，实机可能反馈250 |
|
||
| 右手小指 PIP 电机19，命令0 | ±5 u8，实机可能反馈5 |
|
||
|
||
运动保护：
|
||
|
||
- 单方向扫描超时：90 秒；
|
||
- 连续 8 秒没有至少 1 u8 的目标方向进展：立即暂停并保持当前位置；
|
||
- 机械停滞不消耗遮挡/采样自动重试预算；
|
||
- 发现摩擦、碰撞或仍在变化的反馈时，不要反复调用 `resume` 强推；
|
||
- 只有确认是稳定固件端点时，才允许为该电机、该端点配置专用容差。
|
||
|
||
## 10. 标定前准备
|
||
|
||
### 10.1 构建和加载环境
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
|
||
source /opt/ros/jazzy/setup.bash
|
||
colcon build --symlink-install \
|
||
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
|
||
source install/setup.bash
|
||
```
|
||
|
||
每次修改代码并重新构建后，必须关闭旧标定进程，在新终端重新 `source install/setup.bash` 后启动。
|
||
|
||
### 10.2 现场检查
|
||
|
||
开始前确认：
|
||
|
||
- `can0` 已启动；
|
||
- MVS 客户端没有占用三台相机；
|
||
- 三个内参文件和外参文件对应当前相机安装；
|
||
- 11 个 Tag 固定、平整、全行程可见；
|
||
- 机械手全行程没有碰撞；
|
||
- 没有其他节点向同一只手发布位置命令；
|
||
- 准备好随时断开电机电源；
|
||
- 源 URDF 是原始 CAD 文件。
|
||
|
||
## 11. 禁止运动预检
|
||
|
||
### 11.1 右手预检
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 launch g20_thumb_apriltag_calibration \
|
||
three_camera_calibration.launch.py \
|
||
hand_type:=right \
|
||
serial_number:=G20_RIGHT_001 \
|
||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
|
||
can_interface:=can0 \
|
||
commands_enabled:=false
|
||
```
|
||
|
||
### 11.2 左手预检
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 launch g20_thumb_apriltag_calibration \
|
||
three_camera_calibration.launch.py \
|
||
hand_type:=left \
|
||
serial_number:=G20_LEFT_001 \
|
||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||
can_interface:=can0 \
|
||
commands_enabled:=false
|
||
```
|
||
|
||
`commands_enabled:=false` 时不允许标定位置运动。完成检查后应关闭预检进程，再启动正式流程，避免相机和 SDK 被两个进程同时占用。
|
||
|
||
## 12. 正式标定操作
|
||
|
||
### 12.1 右手正式启动
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 launch g20_thumb_apriltag_calibration \
|
||
three_camera_calibration.launch.py \
|
||
hand_type:=right \
|
||
serial_number:=G20_RIGHT_001 \
|
||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
|
||
can_interface:=can0
|
||
```
|
||
|
||
### 12.2 左手正式启动
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 launch g20_thumb_apriltag_calibration \
|
||
three_camera_calibration.launch.py \
|
||
hand_type:=left \
|
||
serial_number:=G20_LEFT_001 \
|
||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||
can_interface:=can0
|
||
```
|
||
|
||
### 12.3 查看状态
|
||
|
||
另开一个终端：
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 topic echo --once \
|
||
/g20_calibration/status_text \
|
||
--field data
|
||
```
|
||
|
||
需要连续监控时去掉 `--once`。
|
||
|
||
查看三个校正画面：
|
||
|
||
```bash
|
||
ros2 run image_view image_view --ros-args \
|
||
--remap image:=/g20_calibration/front/camera/image_rect
|
||
|
||
ros2 run image_view image_view --ros-args \
|
||
--remap image:=/g20_calibration/side/camera/image_rect
|
||
|
||
ros2 run image_view image_view --ros-args \
|
||
--remap image:=/g20_calibration/top/camera/image_rect
|
||
```
|
||
|
||
### 12.4 开始运动
|
||
|
||
只有状态进入“等待开始”，三个机位均显示“就绪”、外参匹配、所需 Tag 无缺失、SDK 正常后，才调用一次：
|
||
|
||
```bash
|
||
ros2 service call \
|
||
/g20_calibration/start \
|
||
std_srvs/srv/Trigger {}
|
||
```
|
||
|
||
不要重复调用 `start`。程序会自动完成基准姿态、42 个扫描方向、拟合、第三轮留出验证、正式 JSON 和 URDF 写入。
|
||
|
||
## 13. 暂停、恢复和终止
|
||
|
||
### 13.1 手工暂停
|
||
|
||
```bash
|
||
ros2 service call \
|
||
/g20_calibration/pause \
|
||
std_srvs/srv/Trigger {}
|
||
```
|
||
|
||
暂停会保持当前实际位置，不会自动返回基准。
|
||
|
||
### 13.2 恢复可恢复故障
|
||
|
||
```bash
|
||
ros2 service call \
|
||
/g20_calibration/resume \
|
||
std_srvs/srv/Trigger {}
|
||
```
|
||
|
||
恢复规则：
|
||
|
||
- 只要求当前活动机位恢复就绪；
|
||
- 当前失败方向会从起点完整重扫；
|
||
- 关节拟合失败会清除当前失败关节数据并重扫该关节的 6 个方向；
|
||
- 已经通过的其他关节数据保留；
|
||
- 不要用 `start` 代替 `resume`。
|
||
|
||
### 13.3 终止
|
||
|
||
```bash
|
||
ros2 service call \
|
||
/g20_calibration/abort \
|
||
std_srvs/srv/Trigger {}
|
||
```
|
||
|
||
终止会停止任务并保持当前位置，不主动移动机械手。
|
||
|
||
## 14. 自动重试和失败分类
|
||
|
||
| 失败类型 | 程序行为 | 操作建议 |
|
||
|---|---|---|
|
||
| 短时 Tag 丢失、同步中断、端点/分箱不足 | 自动重扫当前方向，最多3次 | 修正遮挡后必要时 `resume` |
|
||
| 单轮/单关节轨迹拟合失败 | 优先重扫失败轮次；最多自动重采2轮 | 修正可见性或机械行程后 `resume` |
|
||
| 电机8秒无进展 | 立即保持并暂停，不消耗采样重试 | 先排查机械问题或确认固件端点 |
|
||
| 三轮轴方向或零位离散过大 | 当前关节结束后暂停 | 修正问题后 `resume` 重扫该关节 |
|
||
| 零位触边、父子轴几何不一致、留出无改善 | 稳定模型失败，不自动重扫 | `resume` 被拒绝；修正根因后启动新会话 |
|
||
| 操作员暂停 | 保持当前位置 | 确认安全后 `resume` |
|
||
|
||
自动重试只改变采集速度和保持时间，不放宽最终质量门限。
|
||
|
||
## 15. 常见状态问题
|
||
|
||
### 15.1 预检只有约 3 Hz
|
||
|
||
优先检查：
|
||
|
||
- 是否通过正式 launch 启动，从而加载 Fast DDS 大图共享内存配置；
|
||
- 是否还有旧相机或 MVS 客户端占用设备；
|
||
- `camera_info` 和 `image_raw/image_rect` 是否都接近 30 Hz；
|
||
- 是否重新 `source install/setup.bash`；
|
||
- 是否存在多个图像查看或录制进程造成额外负载。
|
||
|
||
不要通过降低 Tag 有效率门限绕过帧率问题。
|
||
|
||
### 15.2 显示“缺失Tag=无”，但同时有效率不足
|
||
|
||
“当前缺失”只表示最新帧；同时有效率是预检滑动窗口内所有必需 Tag 同帧有效的比例。等待窗口更新，或排查间歇性遮挡、反光、角点质量和 PnP 分支失败。
|
||
|
||
### 15.3 电机目标255、反馈稳定250
|
||
|
||
当前代码只对右手电机10的255端配置 `±5 u8`。其他电机不能因为一次卡滞而放宽。先确认反馈确实稳定在固件端点，且不存在摩擦或碰撞。
|
||
|
||
### 15.4 被动 DIP 轴外残差过大
|
||
|
||
被动关节允许更宽的单轴残差，但仍必须满足跨轮轴方向和第三轮留出。若 Tag、外参均正常，需检查耦合机构是否存在非理想运动、松动或采样过程中 PnP 分支变化；不要直接放宽最终门限。
|
||
|
||
### 15.5 零位/URDF模型验证失败
|
||
|
||
该失败表示当前稳定观测无法由“原始 CAD + 纯关节零位旋转”解释。重复运动通常不会修复，程序会拒绝 `resume`。应检查原始 URDF、Tag所在刚性件、相机外参身份和标定模型后启动新会话。
|
||
|
||
## 16. 输出目录和文件
|
||
|
||
默认会话目录：
|
||
|
||
```text
|
||
calibration_output/<序列号>/<YYYYMMDD_HHMMSS>/
|
||
```
|
||
|
||
主要文件：
|
||
|
||
```text
|
||
raw_samples.jsonl
|
||
g20_<left|right>_<序列号>_calibration.json
|
||
```
|
||
|
||
修正 URDF 默认写入原始 URDF 所在目录：
|
||
|
||
```text
|
||
linkerhand_g20_<left|right>_zero_calibrated_<序列号>_<时间戳>.urdf
|
||
```
|
||
|
||
写入约束：
|
||
|
||
- 每次都从原始 CAD URDF 生成；
|
||
- 采用 `T_original × Rot(axis, offset)`；
|
||
- 只修改通过验收的主动关节 `origin.rpy`；
|
||
- 不修改 `origin.xyz`、`axis.xyz`、mesh、mimic、连杆长度或机械限位；
|
||
- 不覆盖原始 URDF；
|
||
- 失败时不生成正式 JSON 和正式修正 URDF。
|
||
|
||
程序会拒绝名称包含 `zero_calibrated` 或已有校准时间戳特征的源 URDF，防止重复修正。
|
||
|
||
## 17. 使用已有采样离线重放
|
||
|
||
当完整 `raw_samples.jsonl` 已存在时，可以使用当前代码重新拟合和生成新产物，不连接相机、不发送机械手命令。
|
||
|
||
先只读验证：
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||
--output-tag AXIS_FRAME_V3
|
||
```
|
||
|
||
确认报告 `passed: true` 后写入新产物：
|
||
|
||
```bash
|
||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||
--output-tag AXIS_FRAME_V3 \
|
||
--write
|
||
```
|
||
|
||
`--output-tag` 只允许安全文件名字符。离线重放拒绝覆盖已有 JSON、URDF 和报告。
|
||
|
||
离线流程会额外验证：
|
||
|
||
- 原始 URDF 哈希在处理前后不变；
|
||
- 写出的 URDF 等价于求解的运动学修正；
|
||
- URDF 未修改关节平移和机械限位；
|
||
- 将修正 URDF 作为候选模型重新求解后，残余零偏不超过允许值；
|
||
- 正式文件哈希与临时候选文件一致。
|
||
|
||
## 18. 仿真运行
|
||
|
||
加载修正 URDF 后，使用同次标定 JSON 将 20 通道 u8 命令映射为 21 个 URDF 关节角：
|
||
|
||
```bash
|
||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||
source /opt/ros/jazzy/setup.bash
|
||
source install/setup.bash
|
||
|
||
ros2 launch g20_thumb_apriltag_calibration \
|
||
calibrated_joint_state_bridge.launch.py \
|
||
hand_type:=right \
|
||
calibration_file:=/home/lxp/projects/linkerhand_retarget_ros2/calibration_output/G20_RIGHT_001/<时间戳>/g20_right_G20_RIGHT_001_calibration.json
|
||
```
|
||
|
||
默认：
|
||
|
||
- 订阅 `/cb_right_hand_control_cmd`；
|
||
- 发布 `/sim/mujoco/g20/right/joint_state`；
|
||
- 发布值只包含动态 `angle_rad`；
|
||
- 不会再次叠加 URDF 静态零偏。
|
||
|
||
启动前必须停止其他向同一仿真关节话题发布的桥接节点，避免多个发布者同时驱动模型。
|
||
|
||
## 19. 当前右手 V3 样例结果
|
||
|
||
会话：
|
||
|
||
```text
|
||
G20_RIGHT_001/20260811_120146
|
||
```
|
||
|
||
使用当前 V3 掌坐标系锚定逻辑离线重放后：
|
||
|
||
| 关节 | 静态 URDF 修正 |
|
||
|---|---:|
|
||
| `thumb_cmc_roll` | `+3.833362°` |
|
||
| `thumb_cmc_yaw` | `+1.310573°` |
|
||
| `thumb_cmc_pitch` | `+1.896384°` |
|
||
| `thumb_mcp` | `0°`，保留CAD |
|
||
| 四指 MCP roll/pitch、PIP | `0°`，保留CAD |
|
||
|
||
该次 roll 三轮估计为：
|
||
|
||
```text
|
||
3.825454° / 3.866482° / 3.907776°
|
||
```
|
||
|
||
这些值说明当前会话的重复性，不是新标定的固定目标。重新标定应使用新采样独立计算；如果结果明显偏离且无法通过跨轮/留出门限，程序应拒绝生成正式产物。
|
||
|
||
对应 V3 文件：
|
||
|
||
```text
|
||
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/
|
||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_20260811_120146_AXIS_FRAME_V3.urdf
|
||
|
||
calibration_output/G20_RIGHT_001/20260811_120146/
|
||
g20_right_G20_RIGHT_001_calibration_AXIS_FRAME_V3.json
|
||
g20_right_G20_RIGHT_001_offline_validation_AXIS_FRAME_V3.json
|
||
```
|
||
|
||
## 20. 正式验收检查表
|
||
|
||
正式使用新产物前逐项确认：
|
||
|
||
- [ ] 标定使用原始 CAD URDF，而不是旧修正 URDF；
|
||
- [ ] 三个相机序列号、内参身份和外参身份匹配；
|
||
- [ ] 三个机位接近 30 Hz；
|
||
- [ ] 所需 Tag 同时有效率达到 95%；
|
||
- [ ] 42 个扫描方向全部完成；
|
||
- [ ] 三轮轴方向极差、轨迹残差和回差通过；
|
||
- [ ] 第三轮轨迹 MAE/P95 通过；
|
||
- [ ] 非零零位在第三轮显著改善原始 URDF；
|
||
- [ ] 状态为 `COMPLETE`；
|
||
- [ ] 正式 JSON 和修正 URDF 均已生成；
|
||
- [ ] 仿真加载的是新 URDF 和同次 JSON；
|
||
- [ ] 仿真关节话题只有一个发布者；
|
||
- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
|
||
|