O30临时标定提交

2026-08-17 17:55:13 +08:00
parent a609d521a0
commit 57babb966b
21 changed files with 2602 additions and 243 deletions
@@ -0,0 +1,669 @@
+# 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；
+- [ ] 仿真关节话题只有一个发布者；
+- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
+
@@ -0,0 +1,157 @@
+# 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`。