O30临时标定提交

.gitignore

@@ -50,6 +50,7 @@ Thumbs.db
 
 # Runtime and calibration scratch files
 /logs/
+/MvSdkLog/
 *.tmp
 *.log
 *.bak
@@ -61,13 +62,20 @@ Thumbs.db
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
 /calibration_output/
+/config/*_three_camera_extrinsics.yaml
 *.wear_check.json
 *.checkpoint.json
 *.verification.json
 *_mapping_quality.json
 
-# Device-specific robot descriptions derived from local CMC pitch calibration
+# Device-specific robot descriptions derived from local calibration runs
 /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf
 
 # ROS bag / MCAP recordings and CAN captures
 rosbag2_*/

docs/G20三相机零位标定与URDF修正操作说明.md

@@ -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；
+- [ ] 仿真关节话题只有一个发布者；
+- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
+

docs/O30右手三相机标定与URDF修正操作说明.md

@@ -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`。

src/g20_thumb_apriltag_calibration/README.md

@@ -1,6 +1,331 @@
-# G20 左手拇指正面 AprilTag 标定
+# G20 左右手与 O30 右手 AprilTag 标定
 
-该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
+O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
+profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
+[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
+
+## 三机位三维关节轴零位标定（schema v4）
+
+正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
+`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
+贴正；相机和Tag在一次标定中必须固定。默认绑定为：
+
+```text
+front = DB2163742，Tag 0/1/2/3/10
+side  = DB2163749，Tag 4/5/6/7
+top   = DB2163739，Tag 8/9
+```
+
+11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致：
+
+| ID | 机位 | 固定位置/运动件 |
+|---:|---|---|
+| 0 | 正面 | 正面掌壳固定基准 |
+| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
+| 2 | 正面 | 拇指 MCP 后连杆 |
+| 3 | 正面 | 拇指 IP 后末节 |
+| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
+| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
+| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
+| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
+| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
+| 9 | 上面 | 拇指 CMC yaw 运动件 |
+| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
+
+ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
+ID 9 必须在拇指横摆的完整行程中持续可见。
+贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
+
+每台相机必须有独立内参文件：
+
+```text
+~/.ros/camera_info/hikrobot_DB2163742.yaml
+~/.ros/camera_info/hikrobot_DB2163749.yaml
+~/.ros/camera_info/hikrobot_DB2163739.yaml
+```
+
+### 1. 一次性三相机外参
+
+三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用
+`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘：
+
+```bash
+mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_extrinsics.launch.py \
+  output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
+```
+
+启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换
+`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合
+RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、
+同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张
+PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动
+采集，移到新姿态后再自动采下一组。
+
+外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
+一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
+`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
+配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
+单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
+新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
+再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
+放宽。两组均得到
+至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
+
+```bash
+ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
+ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
+ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
+```
+
+采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终
+显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出
+`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过
+1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时
+绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式
+标定也不会运动。
+
+### 2. 预检和正式标定
+
+先使用禁止运动模式检查三个机位、外参、内参和标签：
+
+```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 \
+  commands_enabled:=false
+```
+
+分别查看正式流程的三个画面：
+
+```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
+```
+
+确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
+
+```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
+```
+
+右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
+
+```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 \
+  can_interface:=can0
+```
+
+状态显示三个机位均“就绪”后只调用一次：
+
+```bash
+ros2 topic echo /g20_calibration/status_text
+ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
+```
+
+收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始
+第一条轨迹扫描：
+
+```text
+[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+ 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+```
+
+左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描
+`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对
+四元数的旋转向量投影到三维拟合轴得到。
+轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立
+交叉检查并参与融合。轴线上一点则由整段
+相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接
+当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的
+光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
+严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承
+给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立
+电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留
+原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
+
+零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
+掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
+稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
+进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
+轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
+超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
+Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
+1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
+小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
+
+`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
+被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
+偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
+`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
+`angle_rad[256]` 实测轨迹。
+
+前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
+差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
+95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
+
+单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
+耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
+不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
+
+四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差
+仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用
+完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型
+之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承
+上游轴方向时不重复报告同一项跨轮轴失败。
+
+四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD
+的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成
+约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
+
+四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
+但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
+修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
+按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
+
+7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
+拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
+约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
+原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
+保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
+偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
+会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
+不会放宽正式结果的硬门限。
+
+生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
+`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
+JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
+
+坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
+重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
+端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
+连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
+暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
+完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
+拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
+只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
+失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
+仍保存在 `raw_samples.jsonl`。
+
+每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
+程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
+分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免
+错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立
+重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。
+再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。
+
+左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像，
+将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为
+`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用
+电机1/16（左）或4/19（右），参考指速度10。
+
+右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到
+255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部
+Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节
+的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手
+仍沿用原有正面机位和基准姿态。
+
+标定 `thumb_cmc_yaw`（电机10）时，程序将
+`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
+ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
+电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
+恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
+暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
+`baseline_command_u8` 不变。
+
+右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19
+的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍
+使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判
+为端点到达。
+
+右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端
+使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为
+电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。
+
+三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
+始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
+状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和
+验证完成、正式JSON与URDF成功写入之前不会显示100%。
+
+上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
+和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
+当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
+
+### 3. 输出
+
+通过后生成精简JSON和一个新URDF：
+
+```text
+calibration_output/G20_LEFT_001/<时间戳>/
+  g20_left_G20_LEFT_001_calibration.json
+src/.../g20_left/
+  linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
+
+calibration_output/G20_RIGHT_001/<时间戳>/
+  g20_right_G20_RIGHT_001_calibration.json
+src/.../g20_right/
+  linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
+```
+
+文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
+`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
+指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
+覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
+改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
+图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
+
+完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
+命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
+
+```bash
+python3 -m g20_thumb_apriltag_calibration.offline_replay \
+  calibration_output/G20_RIGHT_001/20260811_120146 \
+  --output-tag AXIS_FRAME_V3 \
+  --write
+```
+
+下面保留原有正面拇指独立标定说明和兼容入口。
+
+### 4. 修正URDF的运行时关节映射
+
+修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节
+`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点
+`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线
+用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节
+`JointState`（包括5个被动关节）：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
+  hand_type:=right \
+  calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
+```
+
+默认订阅 `/cb_right_hand_control_cmd`，发布
+`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
+发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
+因此不会静默退回旧标定。
+
+该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、
+Linker Hand SDK 和标定状态机，
 只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
 终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
 程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
@@ -36,7 +361,8 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维
   `T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
 - 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
   `config/front_tags.yaml` 里检测节点和标定节点的两组数组。
-- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前配置为 `0.010`。
+- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为
+  `16 mm`，因此配置为 `0.016`。
   测量检测角点所围成的正方形边长，不包含外围白色留边。
 - 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
   静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
@@ -50,11 +376,11 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维
 ```bash
 sudo apt-get update
 sudo apt-get install -y \
-  ros-jazzy-realsense2-camera \
-  ros-jazzy-realsense2-description \
   ros-jazzy-image-pipeline \
   ros-jazzy-apriltag-ros \
-  ros-jazzy-apriltag-msgs
+  ros-jazzy-apriltag-msgs \
+  ros-jazzy-camera-calibration \
+  python3-yaml
 
 cd /home/lxp/projects/linkerhand_retarget_ros2
 source /opt/ros/jazzy/setup.bash
@@ -63,6 +389,58 @@ colcon build --symlink-install \
 source install/setup.bash
 ```
 
+相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在：
+
+```text
+/opt/MVS/lib/64/libMvCameraControl.so
+/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py
+```
+
+正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是
+`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举
+顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
+
+`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
+三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量
+加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz，
+大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。
+
+首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从
+`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时
+仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。
+
+先单独启动相机（不会连接机械手，也不会发送关节命令）：
+
+```bash
+ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
+  --remap __ns:=/camera/camera/color \
+  -p serial_number:=DB2163742 \
+  -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
+```
+
+测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：
+
+```bash
+ros2 topic hz /camera/camera/color/camera_info
+ros2 topic hz /camera/camera/color/image_raw
+```
+
+使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按
+实际标定板修改：
+
+```bash
+ros2 run camera_calibration cameracalibrator \
+  --size 8x6 --square 0.020 \
+  --camera_name hikrobot_front_DB2163742 \
+  --ros-args \
+  --remap image:=/camera/camera/color/image_raw \
+  --remap camera/set_camera_info:=/camera/camera/color/set_camera_info
+```
+
+在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会
+原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、
+分辨率和 ROI；改变任何一项都要重新标定。
+
 连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
 `/g20/cb_left_hand_control_cmd` 的程序。
 
@@ -74,6 +452,7 @@ source install/setup.bash
 ```bash
 ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
   serial_number:=G20_LEFT_001 \
+  camera_serial_number:=DB2163742 \
   commands_enabled:=false
 ```
 
@@ -84,6 +463,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
 ```bash
 ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
   serial_number:=G20_LEFT_001 \
+  camera_serial_number:=DB2163742 \
   can_interface:=can0 \
   calibration_speed:=15 \
   continuous_motion_mode:=endpoint \
@@ -92,7 +472,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
   use_roi:=false
 ```
 
-默认关闭 ROI，AprilTag 使用完整的 1280×720 校正画面。查看实际送入 AprilTag
+默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag
 的完整画面：
 
 ```bash
@@ -241,16 +621,16 @@ for name, joint in data["joints"].items():
 ```
 
 如果相机或 SDK 已由外部进程启动，可传
-`start_camera:=false` 或 `start_sdk:=false`。用 `camera_serial_number:=<序列号>`
-可绑定指定 RealSense。
+`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS
+序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。
 
-D405 的彩色流来自 `depth_module`，启动文件会同时设置
-`depth_module.color_profile` 和 `rgb_camera.color_profile`，默认均为
-`1280x720x30`。连续模式默认关闭深度和 rosbag，以减少 USB、CPU 和磁盘负担；
-它们都不参与角度计算。需要完整诊断留档时可增加
-`enable_depth:=true record_bag:=true`。
+海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益
+`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用
+`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定
+曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加
+`record_bag:=true`。
 
-默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
+默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
 `image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
 校正和 AprilTag 组件运行
 在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。

src/g20_thumb_apriltag_calibration/config/fastdds_large_images.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<dds>
+  <profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <transport_descriptors>
+      <transport_descriptor>
+        <transport_id>g20_udp_transport</transport_id>
+        <type>UDPv4</type>
+        <sendBufferSize>10485760</sendBufferSize>
+        <receiveBufferSize>10485760</receiveBufferSize>
+      </transport_descriptor>
+      <transport_descriptor>
+        <transport_id>g20_shm_transport</transport_id>
+        <type>SHM</type>
+        <segment_size>67108864</segment_size>
+        <port_queue_capacity>512</port_queue_capacity>
+        <healthy_check_timeout_ms>1000</healthy_check_timeout_ms>
+      </transport_descriptor>
+    </transport_descriptors>
+
+    <participant
+      profile_name="g20_large_image_participant"
+      is_default_profile="true">
+      <rtps>
+        <userTransports>
+          <transport_id>g20_udp_transport</transport_id>
+          <transport_id>g20_shm_transport</transport_id>
+        </userTransports>
+        <useBuiltinTransports>false</useBuiltinTransports>
+      </rtps>
+    </participant>
+  </profiles>
+</dds>

src/g20_thumb_apriltag_calibration/config/front_tags.yaml

@@ -6,7 +6,7 @@
     # from back-pressuring image_proc's reliable image publisher.
     qos_profile: sensor_data
     family: 36h11
-    size: 0.01
+    size: 0.016
     profile: false
     max_hamming: 0
     detector:
@@ -20,11 +20,11 @@
     tag:
       ids: [0, 1, 2, 3]
       frames: [tag_t0, tag_t3, tag_t4, tag_t5]
-      sizes: [0.010, 0.010, 0.010, 0.010]
+      sizes: [0.016, 0.016, 0.016, 0.016]
 
 g20_thumb_calibration:
   ros__parameters:
     tag_roles: [t0, t3, t4, t5]
     tag_ids: [0, 1, 2, 3]
     tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
-    tag_sizes_m: [0.010, 0.010, 0.010, 0.010]
+    tag_sizes_m: [0.016, 0.016, 0.016, 0.016]

src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml

@@ -1,5 +0,0 @@
-# This is a flat parameter mapping consumed by realsense2_camera/rs_launch.py.
-# Keep image and CameraInfo durability identical so image_transport can
-# synchronize them for image_proc and apriltag_ros.
-color_qos: DEFAULT
-color_info_qos: DEFAULT

src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml

@@ -0,0 +1,132 @@
+/**:
+  ros__parameters:
+    command_topic: /g20/cb_left_hand_control_cmd
+    state_topic: /g20/cb_left_hand_state
+    info_topic: /g20/cb_left_hand_info
+    setting_topic: /g20/cb_hand_setting_cmd
+    front_camera_info_topic: /g20_calibration/front/camera/camera_info
+    front_detections_topic: /g20_calibration/front/apriltag/detections
+    side_camera_info_topic: /g20_calibration/side/camera/camera_info
+    side_detections_topic: /g20_calibration/side/apriltag/detections
+    top_camera_info_topic: /g20_calibration/top/camera/camera_info
+    top_detections_topic: /g20_calibration/top/apriltag/detections
+
+    # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
+    baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    normal_calibration_speed: 15
+    index_roll_calibration_speed: 5
+    index_flex_calibration_speed: 10
+    # O30固件速度0仍很快，标定时使用最低内部速度，并由SDK位置斜坡控制平均速度。
+    o30_internal_speed_u8: 0
+    # O30 SDK位置斜坡完整走过0～255所需时间；仅O30使用，G20不受影响。
+    o30_command_full_range_seconds: 6.0
+    speed_setting_settle_seconds: 0.25
+
+    # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
+    tag_size_m: 0.016
+    repetitions: 3
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    minimum_detection_hz: 15.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+    # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
+    pnp_group_initialization_frames: 8
+    # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
+    pnp_group_normal_alignment_scale_deg: 5.0
+    pnp_group_maximum_normal_alignment_deg: 15.0
+    top_pnp_invalid_reset_seconds: 1.0
+    # 三维位姿必须与实测20通道状态严格按时间戳配对。
+    maximum_state_image_skew_ms: 50.0
+
+    axis_maximum_plane_rms_m: 0.003
+    # 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
+    # 轴线一致性仍沿用严格检查。
+    passive_axis_maximum_plane_rms_m: 0.004
+    axis_maximum_radial_rms_m: 0.003
+    # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
+    axis_maximum_pose_line_rms_m: 0.001
+    # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
+    # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
+    axis_maximum_rotation_circle_difference_deg: 1.0
+    # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
+    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
+    active_maximum_rotation_orthogonal_rms_deg: 2.5
+    passive_maximum_rotation_orthogonal_rms_deg: 7.5
+    zero_maximum_axis_cycle_difference_deg: 0.75
+    # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
+    zero_maximum_axis_cone_mismatch_deg: 5.0
+    zero_maximum_offset_deg: 20.0
+    # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
+    zero_finger_maximum_offset_deg: 3.0
+
+    endpoint_tolerance_u8: 2.0
+    # O30实机在多数目标处会稳定相差最多4；仅O30使用，不改变G20门限。
+    o30_endpoint_tolerance_u8: 4.0
+    # O30拇指CMC侧摆（电机0）低速命令255端实测稳定反馈为247；该端使用±9。
+    o30_thumb_cmc_roll_255_endpoint_tolerance_u8: 9.0
+    # O30食指MCP侧摆（电机2）命令255端实测稳定反馈为248；该端使用±8。
+    o30_index_mcp_roll_255_endpoint_tolerance_u8: 8.0
+    # O30拇指MCP（电机6）命令0端实测稳定反馈为7；该端使用±8。
+    o30_thumb_mcp_zero_endpoint_tolerance_u8: 8.0
+    # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
+    thumb_yaw_zero_endpoint_tolerance_u8: 4.0
+    # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
+    right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
+    # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
+    pinky_pip_zero_endpoint_tolerance_u8: 5.0
+    endpoint_hold_seconds: 0.5
+    baseline_hold_seconds: 0.5
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
+    # 这类故障不进入遮挡/超时的三次自动重扫。
+    motor_stall_timeout_seconds: 8.0
+    motor_stall_minimum_progress_u8: 1.0
+    invalid_timeout_seconds: 3.0
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
+    automatic_sweep_retry_limit: 3
+    # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
+    automatic_fit_retry_limit: 2
+    automatic_motion_retry_limit: 2
+    # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
+    provisional_warning_ratio: 1.25
+    retry_minimum_speed: 3
+    retry_speed_scales: [0.8, 0.6, 0.5]
+    retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
+
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.003
+    trajectory_minimum_arc_deg: 15.0
+    # 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
+    image_trajectory_maximum_radial_rms_px: 2.0
+    image_trajectory_maximum_radial_p95_px: 3.5
+    image_trajectory_minimum_radius_px: 20.0
+    trajectory_maximum_cycle_travel_difference_deg: 3.0
+    passive_maximum_cycle_travel_difference_deg: 10.0
+    maximum_monotonic_correction_deg: 2.0
+    maximum_hysteresis_deg: 5.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    passive_maximum_hysteresis_deg: 7.5
+
+    # 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
+    validation_enabled: false
+    validation_command_count: 3
+    validation_frames: 10
+    validation_seed: 20260804
+    validation_timeout_seconds: 20.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0

src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml

@@ -0,0 +1,62 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7]
+      frames: [side_base, index_mcp, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml

@@ -0,0 +1,62 @@
+/o30_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o30_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7]
+      frames: [side_base, pinky_mcp, pinky_pip, pinky_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+/o30_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py

@@ -51,6 +51,39 @@ def tag_quality_is_valid(
     )
 
 
+def update_pnp_reset_watchdog(
+    *,
+    detection_good: bool,
+    pnp_valid: bool,
+    now: float,
+    invalid_since: float | None,
+    reset_after_seconds: float,
+) -> tuple[float | None, bool]:
+    """Track continuous PnP-only failures and request a throttled reset."""
+    reset_after = float(reset_after_seconds)
+    if reset_after <= 0.0:
+        raise ValueError("reset_after_seconds must be positive")
+    if not detection_good or pnp_valid:
+        return None, False
+    since = float(now) if invalid_since is None else float(invalid_since)
+    if float(now) - since >= reset_after:
+        # Start a new interval so a permanently bad view is not reset on every
+        # frame. The next valid frame clears the interval.
+        return float(now), True
+    return since, False
+
+
+def required_resume_views(active_view: str | None) -> tuple[str, ...]:
+    """Require only the active view on resume; start still checks all views."""
+    all_views = ("front", "side", "top")
+    if active_view is None:
+        return all_views
+    view = str(active_view)
+    if view not in all_views:
+        raise ValueError(f"unknown calibration view: {view}")
+    return (view,)
+
+
 @dataclass(frozen=True)
 class Observation:
     stamp_ns: int

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py

@@ -0,0 +1,358 @@
+"""Visual roll-alignment aid for one G20 calibration camera."""
+
+from __future__ import annotations
+
+from collections import deque
+import math
+import time
+from typing import Any, Mapping, Sequence
+
+from apriltag_msgs.msg import AprilTagDetectionArray
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+
+from .full_hand import VIEW_TAGS
+from .hikrobot_camera import configure_fastdds_large_image_transport
+from .zero_calibration import detect_reference_alignment_line
+
+
+VIEWS = ("front", "side", "top")
+
+
+def summarize_alignment_measurements(
+    measurements: Sequence[Mapping[str, Any] | None],
+) -> dict[str, Any] | None:
+    """Return a median-smoothed physical reference-line measurement."""
+    valid = [measurement for measurement in measurements if measurement]
+    if not valid:
+        return None
+    return {
+        "line_xyxy_px": np.median(
+            np.asarray(
+                [measurement["line_xyxy_px"] for measurement in valid],
+                dtype=float,
+            ),
+            axis=0,
+        ).tolist(),
+        "angle_rad": float(
+            np.median(
+                [float(measurement["angle_rad"]) for measurement in valid]
+            )
+        ),
+        "vertical_offset_px": float(
+            np.median(
+                [
+                    float(measurement["vertical_offset_px"])
+                    for measurement in valid
+                ]
+            )
+        ),
+        "detected_frames": len(valid),
+        "window_frames": len(measurements),
+    }
+
+
+class G20CameraAlignmentView(Node):
+    """Publish a red/blue roll aid based on a physical scene edge."""
+
+    def __init__(self) -> None:
+        """Configure one view without taking ownership of hand commands."""
+        super().__init__("g20_camera_alignment_view")
+        self.declare_parameter("view", "front")
+        view = str(self.get_parameter("view").value).strip().lower()
+        if view not in VIEWS:
+            raise ValueError(f"view must be one of {VIEWS}")
+        self.view = view
+
+        namespace = f"/g20_calibration/{view}"
+        self.required_tag_ids = {
+            int(value) for value in VIEW_TAGS[view].values()
+        }
+        self.declare_parameter("image_topic", f"{namespace}/camera/image_rect")
+        self.declare_parameter(
+            "detections_topic", f"{namespace}/apriltag/detections"
+        )
+        self.declare_parameter("reference_y_ratio", 0.90)
+        self.declare_parameter("roi_y_min_ratio", 0.55)
+        self.declare_parameter("roi_y_max_ratio", 0.98)
+        self.declare_parameter("minimum_line_length_ratio", 0.30)
+        self.declare_parameter("maximum_candidate_angle_deg", 15.0)
+        self.declare_parameter("maximum_alignment_error_deg", 0.5)
+        self.declare_parameter("maximum_vertical_offset_px", 12.0)
+        self.declare_parameter("maximum_hamming", 0)
+        self.declare_parameter("minimum_decision_margin", 20.0)
+        self.declare_parameter("minimum_edge_pixels", 20.0)
+        self.declare_parameter("smoothing_frames", 10)
+        self.declare_parameter("maximum_line_age_seconds", 1.0)
+        self.declare_parameter("maximum_tag_age_seconds", 1.0)
+        self.declare_parameter("maximum_publish_rate_hz", 10.0)
+        self.declare_parameter("output_scale", 0.75)
+
+        def value(name: str) -> Any:
+            return self.get_parameter(name).value
+
+        self.image_topic = str(value("image_topic"))
+        self.detections_topic = str(value("detections_topic"))
+        self.reference_y_ratio = float(value("reference_y_ratio"))
+        self.roi_y_min_ratio = float(value("roi_y_min_ratio"))
+        self.roi_y_max_ratio = float(value("roi_y_max_ratio"))
+        self.minimum_line_length_ratio = float(
+            value("minimum_line_length_ratio")
+        )
+        self.maximum_candidate_angle_rad = math.radians(
+            float(value("maximum_candidate_angle_deg"))
+        )
+        self.maximum_alignment_error_rad = math.radians(
+            float(value("maximum_alignment_error_deg"))
+        )
+        self.maximum_vertical_offset_px = float(
+            value("maximum_vertical_offset_px")
+        )
+        self.maximum_hamming = int(value("maximum_hamming"))
+        self.minimum_decision_margin = float(value("minimum_decision_margin"))
+        self.minimum_edge_pixels = float(value("minimum_edge_pixels"))
+        self.maximum_line_age_seconds = float(
+            value("maximum_line_age_seconds")
+        )
+        self.maximum_tag_age_seconds = float(value("maximum_tag_age_seconds"))
+        self.maximum_publish_rate_hz = float(value("maximum_publish_rate_hz"))
+        self.output_scale = float(value("output_scale"))
+        smoothing_frames = int(value("smoothing_frames"))
+
+        if not (
+            0.0
+            <= self.roi_y_min_ratio
+            < self.reference_y_ratio
+            < self.roi_y_max_ratio
+            <= 1.0
+        ):
+            raise ValueError(
+                "ratios must satisfy 0 <= roi_min < reference < roi_max <= 1"
+            )
+        if not 0.0 < self.minimum_line_length_ratio <= 1.0:
+            raise ValueError("minimum_line_length_ratio must be in (0, 1]")
+        if not (
+            0.0
+            < self.maximum_alignment_error_rad
+            < self.maximum_candidate_angle_rad
+            < math.pi / 2.0
+        ):
+            raise ValueError(
+                "angle limits must satisfy 0 < alignment < candidate < 90"
+            )
+        if self.maximum_vertical_offset_px <= 0.0:
+            raise ValueError("maximum_vertical_offset_px must be positive")
+        if smoothing_frames < 1:
+            raise ValueError("smoothing_frames must be positive")
+        if self.maximum_line_age_seconds <= 0.0:
+            raise ValueError("maximum_line_age_seconds must be positive")
+        if self.maximum_tag_age_seconds <= 0.0:
+            raise ValueError("maximum_tag_age_seconds must be positive")
+        if self.maximum_publish_rate_hz <= 0.0:
+            raise ValueError("maximum_publish_rate_hz must be positive")
+        if not 0.1 <= self.output_scale <= 1.0:
+            raise ValueError("output_scale must be in [0.1, 1.0]")
+
+        self.bridge = CvBridge()
+        self.line_history: deque[dict[str, Any] | None] = deque(
+            maxlen=smoothing_frames
+        )
+        self.last_line_at = 0.0
+        self.latest_tag_corners: dict[int, np.ndarray] = {}
+        self.latest_tag_at: dict[int, float] = {}
+        self.last_publish_at = 0.0
+        self.publisher = self.create_publisher(
+            Image, "~/image", qos_profile_sensor_data
+        )
+        self.create_subscription(
+            AprilTagDetectionArray,
+            self.detections_topic,
+            self._detections_callback,
+            qos_profile_sensor_data,
+        )
+        self.create_subscription(
+            Image,
+            self.image_topic,
+            self._image_callback,
+            qos_profile_sensor_data,
+        )
+        self.get_logger().info(
+            f"{view} alignment view uses physical long-edge detection; "
+            f"Tag orientation is ignored; input={self.image_topic}; "
+            f"output={self.get_name()}/image"
+        )
+
+    def _detections_callback(self, message: AprilTagDetectionArray) -> None:
+        now = time.monotonic()
+        for detection in message.detections:
+            tag_id = int(detection.id)
+            if tag_id not in self.required_tag_ids:
+                continue
+            corners = np.asarray(
+                [
+                    [float(point.x), float(point.y)]
+                    for point in detection.corners
+                ],
+                dtype=float,
+            )
+            if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+                continue
+            edges = np.linalg.norm(
+                corners - np.roll(corners, -1, axis=0), axis=1
+            )
+            if (
+                int(detection.hamming) > self.maximum_hamming
+                or float(detection.decision_margin)
+                < self.minimum_decision_margin
+                or float(np.mean(edges)) < self.minimum_edge_pixels
+            ):
+                continue
+            self.latest_tag_corners[tag_id] = corners
+            self.latest_tag_at[tag_id] = now
+
+    def _draw_tags(self, image: np.ndarray, now: float) -> None:
+        for tag_id in sorted(self.required_tag_ids):
+            corners = self.latest_tag_corners.get(tag_id)
+            detected_at = self.latest_tag_at.get(tag_id, 0.0)
+            if (
+                corners is None
+                or now - detected_at > self.maximum_tag_age_seconds
+            ):
+                continue
+            points = np.rint(corners * self.output_scale).astype(np.int32)
+            cv2.polylines(image, [points], True, (0, 220, 0), 2)
+            centre = np.rint(np.mean(points, axis=0)).astype(int)
+            cv2.putText(
+                image,
+                f"ID {tag_id}",
+                (int(centre[0]) + 5, int(centre[1]) - 7),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.55,
+                (0, 220, 0),
+                2,
+            )
+
+    def _image_callback(self, message: Image) -> None:
+        # Avoid conversion and Hough work until an image viewer subscribes.
+        if self.publisher.get_subscription_count() < 1:
+            return
+        now = time.monotonic()
+        if now - self.last_publish_at < 1.0 / self.maximum_publish_rate_hz:
+            return
+        self.last_publish_at = now
+        try:
+            image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except Exception as error:
+            self.get_logger().warning(
+                f"alignment image conversion failed: {error}"
+            )
+            return
+        if self.output_scale != 1.0:
+            image = cv2.resize(
+                image,
+                None,
+                fx=self.output_scale,
+                fy=self.output_scale,
+                interpolation=cv2.INTER_AREA,
+            )
+
+        height, width = image.shape[:2]
+        reference_y = self.reference_y_ratio * float(height - 1)
+        detected = detect_reference_alignment_line(
+            image,
+            reference_y_px=reference_y,
+            roi_y_min_ratio=self.roi_y_min_ratio,
+            roi_y_max_ratio=self.roi_y_max_ratio,
+            minimum_length_ratio=self.minimum_line_length_ratio,
+            maximum_candidate_angle_rad=self.maximum_candidate_angle_rad,
+        )
+        self.line_history.append(detected)
+        if detected is not None:
+            self.last_line_at = now
+        measurement = summarize_alignment_measurements(self.line_history)
+        if now - self.last_line_at > self.maximum_line_age_seconds:
+            measurement = None
+
+        red_y = int(round(reference_y))
+        cv2.line(
+            image,
+            (15, red_y),
+            (max(15, width - 15), red_y),
+            (0, 0, 255),
+            4,
+        )
+        if measurement is not None:
+            line = np.rint(measurement["line_xyxy_px"]).astype(int)
+            blue_ok, blue_start, blue_end = cv2.clipLine(
+                (0, 0, width, height),
+                (int(line[0]), int(line[1])),
+                (int(line[2]), int(line[3])),
+            )
+            if blue_ok:
+                cv2.line(image, blue_start, blue_end, (255, 0, 0), 3)
+            angle_rad = float(measurement["angle_rad"])
+            offset_px = float(measurement["vertical_offset_px"])
+            aligned = bool(
+                abs(angle_rad) <= self.maximum_alignment_error_rad
+                and abs(offset_px) <= self.maximum_vertical_offset_px
+            )
+            status = "ALIGNED" if aligned else "ADJUST CAMERA"
+            status_text = (
+                f"{self.view.upper()}  red-blue "
+                f"{math.degrees(angle_rad):+.2f} deg  "
+                f"dy {offset_px:+.1f}px  {status}"
+            )
+            status_color = (0, 220, 0) if aligned else (0, 165, 255)
+        else:
+            status_text = (
+                f"{self.view.upper()}  PHYSICAL REFERENCE LINE NOT DETECTED"
+            )
+            status_color = (0, 165, 255)
+        cv2.putText(
+            image,
+            status_text,
+            (20, 34),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.72,
+            status_color,
+            2,
+        )
+        self._draw_tags(image, now)
+        cv2.putText(
+            image,
+            "RED=target  BLUE=physical edge  GREEN=Tags (angle ignored)",
+            (20, max(64, height - 24)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.60,
+            (255, 255, 255),
+            2,
+        )
+        output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
+        output.header = message.header
+        self.publisher.publish(output)
+
+
+def main(args: list[str] | None = None) -> None:
+    """Run the single-view alignment helper."""
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: G20CameraAlignmentView | None = None
+    try:
+        node = G20CameraAlignmentView()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,288 @@
+"""Map supported-hand u8 commands to URDF angles using calibration JSON.
+
+The static encoder-zero corrections in ``zero_angles`` are already baked into
+the corrected URDF joint origins.  This bridge therefore publishes only the
+dynamic ``angle_rad`` values and never adds the static offsets a second time.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .full_hand import get_hand_calibration_profile, validate_compact_payload
+
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+# Match the stable ordering used by the existing MuJoCo bridge.  JointState
+# consumers must use names, but retaining the ordering also keeps logs and
+# direct comparisons deterministic.
+G20_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "index_dip",
+    "index_mcp_pitch",
+    "index_mcp_roll",
+    "index_pip",
+    "middle_dip",
+    "middle_mcp_pitch",
+    "middle_mcp_roll",
+    "middle_pip",
+    "pinky_dip",
+    "pinky_mcp_pitch",
+    "pinky_mcp_roll",
+    "pinky_pip",
+    "ring_dip",
+    "ring_mcp_pitch",
+    "ring_mcp_roll",
+    "ring_pip",
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_ip",
+    "thumb_mcp",
+)
+
+O30_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_roll",
+    "thumb_yaw",
+    "index_yaw",
+    "middle_yaw",
+    "ring_yaw",
+    "little_yaw",
+    "thumb_root1",
+    "index_root1",
+    "middle_root1",
+    "ring_root1",
+    "little_root1",
+    "index_root2",
+    "middle_root2",
+    "ring_root2",
+    "little_root2",
+    "thumb_tip",
+    "index_tip",
+    "middle_tip",
+    "ring_tip",
+    "little_tip",
+)
+
+O30_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_mcp",
+    "thumb_ip",
+    "index_mcp_roll",
+    "index_mcp_pitch",
+    "index_pip",
+    "index_dip",
+    "middle_mcp_roll",
+    "middle_mcp_pitch",
+    "middle_pip",
+    "middle_dip",
+    "ring_mcp_roll",
+    "ring_mcp_pitch",
+    "ring_pip",
+    "ring_dip",
+    "pinky_mcp_roll",
+    "pinky_mcp_pitch",
+    "pinky_pip",
+    "pinky_dip",
+)
+
+COMMAND_NAMES_BY_MODEL = {
+    "G20": G20_COMMAND_NAMES,
+    "O30": O30_COMMAND_NAMES,
+}
+URDF_JOINT_NAMES_BY_MODEL = {
+    "G20": G20_URDF_JOINT_NAMES,
+    "O30": O30_URDF_JOINT_NAMES,
+}
+
+
+class CalibratedCommandMapper:
+    """Validated, model/side-specific lookup from commands to URDF radians."""
+
+    def __init__(
+        self, payload: Mapping[str, Any], *, expected_side: str | None = None
+    ) -> None:
+        validate_compact_payload(payload)
+        side = str(payload["side"]).lower()
+        if expected_side is not None and side != str(expected_side).lower():
+            raise ValueError(
+                f"calibration side {side!r} does not match requested side "
+                f"{str(expected_side).lower()!r}"
+            )
+        quality = payload["quality"]
+        if quality.get("passed") is not True:
+            raise ValueError("calibration quality.passed must be true")
+        model = str(payload["model"]).upper()
+        profile = get_hand_calibration_profile(side, model)
+        urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model]
+        self.model = model
+        self.side = side
+        self.serial_number = str(payload["serial_number"])
+        self.command_names = COMMAND_NAMES_BY_MODEL[model]
+        self.urdf_joint_names = urdf_joint_names
+        self._motor_by_joint = {
+            name: int(profile.joint_specs[name].motor_index)
+            for name in urdf_joint_names
+        }
+        self._curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name]["angle_rad"]
+            )
+            for name in urdf_joint_names
+        }
+
+    @staticmethod
+    def _command_index(value: float) -> int:
+        command = float(value)
+        if not math.isfinite(command):
+            raise ValueError("command positions must be finite")
+        return max(0, min(255, int(math.floor(command + 0.5))))
+
+    def map_positions(
+        self, positions: Sequence[float], names: Sequence[str] = ()
+    ) -> tuple[float, ...]:
+        values = tuple(float(value) for value in positions)
+        if names:
+            if len(names) != len(values):
+                raise ValueError(
+                    "JointState names and positions must have equal length"
+                )
+            if len(set(names)) != len(names):
+                raise ValueError("JointState names must be unique")
+            by_name = dict(zip((str(name) for name in names), values))
+            missing = [name for name in self.command_names if name not in by_name]
+            if missing:
+                raise ValueError(
+                    f"{self.model} command is missing named channels: "
+                    + ",".join(missing)
+                )
+            command = tuple(by_name[name] for name in self.command_names)
+        else:
+            if len(values) != len(self.command_names):
+                raise ValueError(
+                    f"unnamed {self.model} command must contain exactly "
+                    f"{len(self.command_names)} positions"
+                )
+            command = values
+        indices = tuple(self._command_index(value) for value in command)
+        return tuple(
+            self._curves[name][indices[self._motor_by_joint[name]]]
+            for name in self.urdf_joint_names
+        )
+
+
+def load_calibrated_command_mapper(
+    calibration_file: str | Path, *, expected_side: str | None = None
+) -> CalibratedCommandMapper:
+    path = Path(calibration_file).expanduser().resolve()
+    if not path.is_file():
+        raise ValueError(f"calibration JSON does not exist: {path}")
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    return CalibratedCommandMapper(payload, expected_side=expected_side)
+
+
+class CalibratedJointStateBridge(Node):
+    def __init__(self) -> None:
+        super().__init__("g20_calibrated_joint_state_bridge")
+        self.declare_parameter("hand_model", "G20")
+        self.declare_parameter("hand_type", "right")
+        self.declare_parameter("calibration_file", "")
+        self.declare_parameter("input_topic", "")
+        self.declare_parameter("output_topic", "")
+
+        hand_model = str(self.get_parameter("hand_model").value).upper()
+        hand_type = str(self.get_parameter("hand_type").value).lower()
+        get_hand_calibration_profile(hand_type, hand_model)
+        calibration_file = str(self.get_parameter("calibration_file").value)
+        if not calibration_file:
+            raise ValueError("calibration_file is required")
+        self.mapper = load_calibrated_command_mapper(
+            calibration_file, expected_side=hand_type
+        )
+        if self.mapper.model != hand_model:
+            raise ValueError(
+                f"calibration model {self.mapper.model!r} does not match "
+                f"requested model {hand_model!r}"
+            )
+        input_topic = str(self.get_parameter("input_topic").value).strip()
+        output_topic = str(self.get_parameter("output_topic").value).strip()
+        self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
+        self.output_topic = (
+            output_topic
+            or f"/sim/mujoco/{hand_model.lower()}/{hand_type}/joint_state"
+        )
+        self.publisher = self.create_publisher(JointState, self.output_topic, 10)
+        self.subscription = self.create_subscription(
+            JointState, self.input_topic, self._command_callback, 10
+        )
+        self._last_error = ""
+        self.get_logger().info(
+            f"loaded {hand_type} {hand_model} calibration for "
+            f"{self.mapper.serial_number}: "
+            f"{self.input_topic} -> {self.output_topic}"
+        )
+
+    def _command_callback(self, command: JointState) -> None:
+        try:
+            positions = self.mapper.map_positions(command.position, command.name)
+        except ValueError as error:
+            message = str(error)
+            if message != self._last_error:
+                self.get_logger().error(message)
+                self._last_error = message
+            return
+        self._last_error = ""
+        result = JointState()
+        result.header = command.header
+        result.name = list(self.mapper.urdf_joint_names)
+        result.position = list(positions)
+        self.publisher.publish(result)
+
+
+def main(args: Sequence[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: CalibratedJointStateBridge | None = None
+    try:
+        node = CalibratedJointStateBridge()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics.py

@@ -0,0 +1,227 @@
+"""Camera-extrinsic data model shared by calibration and runtime nodes."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation
+
+
+VIEWS: tuple[str, ...] = ("front", "side", "top")
+
+
+def camera_info_fingerprint(
+    *,
+    width: int,
+    height: int,
+    camera_matrix: Sequence[Sequence[float]] | Sequence[float],
+    distortion: Sequence[float] = (),
+    rectification: Sequence[float] = (),
+    projection: Sequence[float] = (),
+) -> str:
+    """Return a stable fingerprint for rectified image geometry."""
+    matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
+    payload = {
+        "width": int(width),
+        "height": int(height),
+        "camera_matrix": [round(float(value), 12) for value in matrix.flat],
+        "distortion": [round(float(value), 12) for value in distortion],
+        "rectification": [round(float(value), 12) for value in rectification],
+        "projection": [round(float(value), 12) for value in projection],
+    }
+    encoded = json.dumps(
+        payload, sort_keys=True, separators=(",", ":")
+    ).encode("utf-8")
+    return hashlib.sha256(encoded).hexdigest()
+
+
+def transform_matrix(
+    translation_xyz_m: Sequence[float],
+    quaternion_xyzw: Sequence[float],
+) -> np.ndarray:
+    translation = np.asarray(translation_xyz_m, dtype=float)
+    quaternion = np.asarray(quaternion_xyzw, dtype=float)
+    if translation.shape != (3,) or not np.all(np.isfinite(translation)):
+        raise ValueError("translation_xyz_m must contain three finite values")
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion_xyzw must contain four finite values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1.0e-12:
+        raise ValueError("quaternion_xyzw has zero norm")
+    result = np.eye(4, dtype=float)
+    result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
+    result[:3, 3] = translation
+    return result
+
+
+def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
+    value = np.asarray(matrix, dtype=float)
+    if value.shape != (4, 4) or not np.all(np.isfinite(value)):
+        raise ValueError("transform must be a finite 4x4 matrix")
+    return {
+        "translation_xyz_m": [float(item) for item in value[:3, 3]],
+        "quaternion_xyzw": [
+            float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
+        ],
+    }
+
+
+@dataclass(frozen=True)
+class CameraCalibrationIdentity:
+    serial_number: str
+    width: int
+    height: int
+    intrinsics_sha256: str
+
+
+@dataclass(frozen=True)
+class ThreeCameraExtrinsics:
+    """Transforms points from each camera optical frame into front optical."""
+
+    cameras: Mapping[str, CameraCalibrationIdentity]
+    front_from_view: Mapping[str, np.ndarray]
+    quality: Mapping[str, float]
+
+    def transform(self, view: str) -> np.ndarray:
+        if view not in self.front_from_view:
+            raise KeyError(f"extrinsics do not contain view {view}")
+        return np.asarray(self.front_from_view[view], dtype=float).copy()
+
+    def camera_matches(
+        self,
+        view: str,
+        *,
+        serial_number: str,
+        width: int,
+        height: int,
+        intrinsics_sha256: str,
+    ) -> bool:
+        expected = self.cameras.get(view)
+        return bool(
+            expected is not None
+            and expected.serial_number == str(serial_number)
+            and expected.width == int(width)
+            and expected.height == int(height)
+            and expected.intrinsics_sha256 == str(intrinsics_sha256)
+        )
+
+
+def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
+    if int(payload.get("schema_version", -1)) != 1:
+        raise ValueError("camera extrinsics schema_version must be 1")
+    if payload.get("reference_view") != "front":
+        raise ValueError("camera extrinsics reference_view must be front")
+    cameras = payload.get("cameras")
+    transforms = payload.get("front_from_view")
+    quality = payload.get("quality")
+    if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
+        raise ValueError("camera extrinsics must contain front/side/top cameras")
+    if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
+        raise ValueError("camera extrinsics must contain all three transforms")
+    if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
+        raise ValueError("camera extrinsics quality is not passed")
+    quality_limits = {
+        "reprojection_rms_px": 1.2,
+        "maximum_rotation_repeatability_deg": 0.3,
+        "maximum_translation_repeatability_m": 0.0015,
+    }
+    for key, limit in quality_limits.items():
+        value = float(quality.get(key, float("inf")))
+        if not np.isfinite(value) or value > limit:
+            raise ValueError(
+                f"camera extrinsics {key}={value} exceeds {limit}"
+            )
+    for key in ("front_side_captures", "front_top_captures"):
+        if int(quality.get(key, 0)) < 15:
+            raise ValueError(f"camera extrinsics {key} must be at least 15")
+    for view in VIEWS:
+        identity = cameras[view]
+        if not isinstance(identity, Mapping):
+            raise ValueError(f"{view} camera identity must be an object")
+        if not str(identity.get("serial_number", "")):
+            raise ValueError(f"{view} camera serial_number is missing")
+        if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
+            raise ValueError(f"{view} camera image dimensions are invalid")
+        fingerprint = str(identity.get("intrinsics_sha256", ""))
+        if len(fingerprint) != 64:
+            raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
+        transform = transforms[view]
+        if not isinstance(transform, Mapping):
+            raise ValueError(f"{view} transform must be an object")
+        matrix = transform_matrix(
+            transform.get("translation_xyz_m", ()),
+            transform.get("quaternion_xyzw", ()),
+        )
+        if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
+            raise ValueError("front_from_view.front must be identity")
+    serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
+    if len(set(serials)) != len(VIEWS):
+        raise ValueError("camera extrinsics serial numbers must be unique")
+
+
+def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
+    source = Path(path).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"camera extrinsics file does not exist: {source}")
+    with source.open("r", encoding="utf-8") as stream:
+        payload = yaml.safe_load(stream)
+    if not isinstance(payload, Mapping):
+        raise ValueError("camera extrinsics file must contain an object")
+    validate_extrinsics_payload(payload)
+    cameras = {
+        view: CameraCalibrationIdentity(
+            serial_number=str(payload["cameras"][view]["serial_number"]),
+            width=int(payload["cameras"][view]["width"]),
+            height=int(payload["cameras"][view]["height"]),
+            intrinsics_sha256=str(
+                payload["cameras"][view]["intrinsics_sha256"]
+            ),
+        )
+        for view in VIEWS
+    }
+    transforms = {
+        view: transform_matrix(
+            payload["front_from_view"][view]["translation_xyz_m"],
+            payload["front_from_view"][view]["quaternion_xyzw"],
+        )
+        for view in VIEWS
+    }
+    return ThreeCameraExtrinsics(
+        cameras=cameras,
+        front_from_view=transforms,
+        quality={
+            str(key): float(value) if isinstance(value, (int, float)) else value
+            for key, value in payload["quality"].items()
+        },
+    )
+
+
+def dump_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    cameras: Mapping[str, Mapping[str, Any]],
+    front_from_view: Mapping[str, Sequence[Sequence[float]]],
+    quality: Mapping[str, Any],
+) -> None:
+    payload = {
+        "schema_version": 1,
+        "reference_view": "front",
+        "cameras": {view: dict(cameras[view]) for view in VIEWS},
+        "front_from_view": {
+            view: matrix_payload(front_from_view[view]) for view in VIEWS
+        },
+        "quality": dict(quality),
+    }
+    validate_extrinsics_payload(payload)
+    destination = Path(path).expanduser().resolve()
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    with temporary.open("w", encoding="utf-8") as stream:
+        yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
+    temporary.replace(destination)

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py

@@ -0,0 +1,644 @@
+"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
+
+from __future__ import annotations
+
+from ctypes import byref, cast, memset, POINTER, sizeof, string_at
+from dataclasses import dataclass
+import importlib
+import os
+from pathlib import Path
+import sys
+from threading import Event, Lock, Thread
+from typing import Any, Sequence
+from urllib.parse import unquote, urlparse
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+from sensor_msgs.srv import SetCameraInfo
+import yaml
+
+
+DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
+DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
+
+
+@dataclass(frozen=True)
+class DeviceDescriptor:
+    """Stable identity fields for one enumerated USB3 Vision camera."""
+
+    index: int
+    model: str
+    serial: str
+    guid: str
+
+
+@dataclass(frozen=True)
+class CameraCalibration:
+    """Validated subset of a standard ROS camera calibration YAML file."""
+
+    camera_name: str
+    width: int
+    height: int
+    distortion_model: str
+    d: tuple[float, ...]
+    k: tuple[float, ...]
+    r: tuple[float, ...]
+    p: tuple[float, ...]
+
+
+def decode_c_string(value: Any) -> str:
+    """Decode a null-terminated MVS SDK character array."""
+    raw = bytes(value).split(b"\0", 1)[0]
+    for encoding in ("utf-8", "gbk", "latin-1"):
+        try:
+            return raw.decode(encoding)
+        except UnicodeDecodeError:
+            continue
+    return raw.decode("latin-1", errors="replace")
+
+
+def select_device(
+    devices: Sequence[DeviceDescriptor],
+    selector: str,
+    expected_model: str,
+) -> DeviceDescriptor:
+    """Select exactly one camera by serial or GUID and check its model."""
+    if not devices:
+        raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
+    selector = selector.strip()
+    if selector:
+        matches = [
+            device
+            for device in devices
+            if selector in {device.serial, device.guid}
+        ]
+        if not matches:
+            available = ", ".join(
+                f"{device.model}:{device.serial}:{device.guid}"
+                for device in devices
+            )
+            raise RuntimeError(
+                f"camera selector {selector!r} was not found; "
+                f"available cameras: {available}"
+            )
+    elif len(devices) == 1:
+        matches = [devices[0]]
+    else:
+        available = ", ".join(device.serial for device in devices)
+        raise RuntimeError(
+            "camera selector is required when multiple cameras are present; "
+            f"available serials: {available}"
+        )
+    if len(matches) != 1:
+        raise RuntimeError(f"camera selector {selector!r} is not unique")
+    selected = matches[0]
+    if expected_model and expected_model not in selected.model:
+        raise RuntimeError(
+            f"camera {selected.serial} is {selected.model!r}, expected a model "
+            f"containing {expected_model!r}"
+        )
+    return selected
+
+
+def resolve_camera_info_path(url_or_path: str) -> Path | None:
+    """Resolve a plain path or file:// URL used for CameraInfo persistence."""
+    value = url_or_path.strip()
+    if not value:
+        return None
+    parsed = urlparse(value)
+    if parsed.scheme not in {"", "file"}:
+        raise ValueError(
+            "camera_info_url must be a filesystem path or file:// URL"
+        )
+    if parsed.scheme == "file":
+        if parsed.netloc not in {"", "localhost"}:
+            raise ValueError("remote file:// camera_info_url is not supported")
+        value = unquote(parsed.path)
+    return Path(value).expanduser().resolve()
+
+
+def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
+    section = mapping.get(name)
+    if not isinstance(section, dict):
+        raise ValueError(f"camera calibration is missing {name}")
+    data = section.get("data")
+    if not isinstance(data, list) or len(data) != length:
+        raise ValueError(f"{name}.data must contain exactly {length} values")
+    result = tuple(float(value) for value in data)
+    if not all(value == value and abs(value) != float("inf") for value in result):
+        raise ValueError(f"{name}.data contains a non-finite value")
+    return result
+
+
+def load_camera_calibration(path: Path) -> CameraCalibration:
+    """Load and validate a standard ROS camera calibration YAML file."""
+    mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
+    if not isinstance(mapping, dict):
+        raise ValueError("camera calibration YAML root must be a mapping")
+    width = int(mapping.get("image_width", 0))
+    height = int(mapping.get("image_height", 0))
+    if width <= 0 or height <= 0:
+        raise ValueError("camera calibration image dimensions must be positive")
+    distortion = mapping.get("distortion_coefficients")
+    if not isinstance(distortion, dict) or not isinstance(
+        distortion.get("data"), list
+    ):
+        raise ValueError(
+            "camera calibration is missing distortion_coefficients.data"
+        )
+    d = tuple(float(value) for value in distortion["data"])
+    calibration = CameraCalibration(
+        camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
+        width=width,
+        height=height,
+        distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
+        d=d,
+        k=_matrix_data(mapping, "camera_matrix", 9),
+        r=_matrix_data(mapping, "rectification_matrix", 9),
+        p=_matrix_data(mapping, "projection_matrix", 12),
+    )
+    if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
+        raise ValueError("camera calibration focal lengths must be positive")
+    if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
+        raise ValueError("camera calibration projection focal lengths must be positive")
+    return calibration
+
+
+def calibration_to_mapping(
+    camera_name: str, camera_info: CameraInfo
+) -> dict[str, Any]:
+    """Convert CameraInfo into the standard ROS YAML representation."""
+    return {
+        "image_width": int(camera_info.width),
+        "image_height": int(camera_info.height),
+        "camera_name": camera_name,
+        "camera_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.k],
+        },
+        "distortion_model": camera_info.distortion_model,
+        "distortion_coefficients": {
+            "rows": 1,
+            "cols": len(camera_info.d),
+            "data": [float(value) for value in camera_info.d],
+        },
+        "rectification_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.r],
+        },
+        "projection_matrix": {
+            "rows": 3,
+            "cols": 4,
+            "data": [float(value) for value in camera_info.p],
+        },
+    }
+
+
+def configure_fastdds_large_image_transport() -> Path:
+    """Select the package's large-image SHM profile before creating ROS nodes."""
+    from ament_index_python.packages import get_package_share_directory
+
+    profile = (
+        Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
+        / "config"
+        / "fastdds_large_images.xml"
+    )
+    if not profile.is_file():
+        raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
+    os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
+    return profile
+
+
+class HikrobotCameraNode(Node):
+    """Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
+
+    def __init__(self) -> None:
+        super().__init__("hikrobot_camera")
+        self._declare_parameters()
+        self._stop_event = Event()
+        self._camera_info_lock = Lock()
+        self._grab_thread: Thread | None = None
+        self._camera: Any | None = None
+        self._mvs: Any | None = None
+        self._sdk_initialized = False
+        self._handle_created = False
+        self._camera_opened = False
+        self._grabbing = False
+        self._closed = False
+        self._frame_count = 0
+        self._timeout_count = 0
+
+        self.selector = str(self.get_parameter("serial_number").value)
+        self.expected_model = str(self.get_parameter("expected_model").value)
+        self.frame_id = str(self.get_parameter("frame_id").value)
+        self.camera_name = str(self.get_parameter("camera_name").value)
+        self.image_width = int(self.get_parameter("image_width").value)
+        self.image_height = int(self.get_parameter("image_height").value)
+        if self.image_width <= 0 or self.image_height <= 0:
+            raise ValueError("image_width and image_height must be positive")
+        if float(self.get_parameter("frame_rate").value) <= 0.0:
+            raise ValueError("frame_rate must be positive")
+        if int(self.get_parameter("grab_timeout_ms").value) <= 0:
+            raise ValueError("grab_timeout_ms must be positive")
+        self.camera_info_path = resolve_camera_info_path(
+            str(self.get_parameter("camera_info_url").value)
+        )
+        self._calibration = self._load_calibration()
+
+        self.image_publisher = self.create_publisher(
+            Image, "image_raw", qos_profile_sensor_data
+        )
+        self.camera_info_publisher = self.create_publisher(
+            CameraInfo, "camera_info", qos_profile_sensor_data
+        )
+        self.create_service(
+            SetCameraInfo, "set_camera_info", self._set_camera_info_callback
+        )
+
+        try:
+            self._open_camera()
+            self._grab_thread = Thread(
+                target=self._grab_loop,
+                name="hikrobot_mvs_grab",
+                daemon=True,
+            )
+            self._grab_thread.start()
+        except Exception:
+            self.close()
+            raise
+
+    def _declare_parameters(self) -> None:
+        self.declare_parameter("serial_number", "")
+        self.declare_parameter("expected_model", "MV-CS020-10UM")
+        self.declare_parameter("camera_name", "hikrobot_front")
+        self.declare_parameter("frame_id", "camera_color_optical_frame")
+        self.declare_parameter("image_width", 1624)
+        self.declare_parameter("image_height", 1240)
+        self.declare_parameter("frame_rate", 30.0)
+        self.declare_parameter("exposure_time_us", 5000.0)
+        self.declare_parameter("gain_db", 0.0)
+        self.declare_parameter("auto_exposure", False)
+        self.declare_parameter("camera_info_url", "")
+        self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
+        self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
+        self.declare_parameter("grab_timeout_ms", 1000)
+
+    def _load_calibration(self) -> CameraCalibration | None:
+        if self.camera_info_path is None or not self.camera_info_path.exists():
+            target = self.camera_info_path or "an unset camera_info_url"
+            self.get_logger().warning(
+                f"No camera calibration found at {target}; publishing "
+                "uncalibrated CameraInfo. Trajectory calibration will remain "
+                "locked until valid intrinsics are installed."
+            )
+            return None
+        calibration = load_camera_calibration(self.camera_info_path)
+        self._validate_calibration_dimensions(calibration)
+        self.get_logger().info(
+            f"Loaded camera intrinsics from {self.camera_info_path}"
+        )
+        return calibration
+
+    def _validate_calibration_dimensions(
+        self, calibration: CameraCalibration
+    ) -> None:
+        if (
+            calibration.width != self.image_width
+            or calibration.height != self.image_height
+        ):
+            raise ValueError(
+                "camera calibration dimensions "
+                f"{calibration.width}x{calibration.height} do not match "
+                f"configured stream {self.image_width}x{self.image_height}"
+            )
+
+    def _import_mvs(self) -> Any:
+        python_path = Path(
+            str(self.get_parameter("mvs_python_path").value)
+        ).expanduser()
+        runtime_path = Path(
+            str(self.get_parameter("mvs_runtime_path").value)
+        ).expanduser()
+        if not python_path.is_dir():
+            raise RuntimeError(f"MVS Python bindings not found: {python_path}")
+        if not runtime_path.is_dir():
+            raise RuntimeError(f"MVS runtime not found: {runtime_path}")
+        os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
+        if str(python_path) not in sys.path:
+            sys.path.insert(0, str(python_path))
+        return importlib.import_module("MvCameraControl_class")
+
+    def _enumerate_usb_cameras(
+        self, mvs: Any
+    ) -> tuple[Any, list[DeviceDescriptor]]:
+        device_list = mvs.MV_CC_DEVICE_INFO_LIST()
+        result = mvs.MvCamera.MV_CC_EnumDevices(
+            mvs.MV_USB_DEVICE, device_list
+        )
+        self._require_ok("enumerate USB3 Vision devices", result)
+        descriptors: list[DeviceDescriptor] = []
+        for index in range(device_list.nDeviceNum):
+            device_info = cast(
+                device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
+            ).contents
+            usb_info = device_info.SpecialInfo.stUsb3VInfo
+            descriptors.append(
+                DeviceDescriptor(
+                    index=index,
+                    model=decode_c_string(usb_info.chModelName),
+                    serial=decode_c_string(usb_info.chSerialNumber),
+                    guid=decode_c_string(usb_info.chDeviceGUID),
+                )
+            )
+        return device_list, descriptors
+
+    def _open_camera(self) -> None:
+        mvs = self._import_mvs()
+        self._mvs = mvs
+        self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
+        self._sdk_initialized = True
+        device_list, descriptors = self._enumerate_usb_cameras(mvs)
+        selected = select_device(descriptors, self.selector, self.expected_model)
+        device_info = cast(
+            device_list.pDeviceInfo[selected.index],
+            POINTER(mvs.MV_CC_DEVICE_INFO),
+        ).contents
+
+        camera = mvs.MvCamera()
+        self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
+        self._camera = camera
+        self._handle_created = True
+        self._require_ok(
+            "open camera",
+            camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
+        )
+        self._camera_opened = True
+
+        self._set_required_enum("AcquisitionMode", "Continuous")
+        self._set_required_enum("TriggerMode", "Off")
+        self._set_required_enum("PixelFormat", "Mono8")
+        self._set_required_integer("OffsetX", 0)
+        self._set_required_integer("OffsetY", 0)
+        self._set_required_integer("Width", self.image_width)
+        self._set_required_integer("Height", self.image_height)
+        self._set_optional_bool("AcquisitionFrameRateEnable", True)
+        self._set_optional_float(
+            "AcquisitionFrameRate",
+            float(self.get_parameter("frame_rate").value),
+        )
+        if bool(self.get_parameter("auto_exposure").value):
+            self._set_required_enum("ExposureAuto", "Continuous")
+        else:
+            self._set_required_enum("ExposureAuto", "Off")
+            self._set_optional_float(
+                "ExposureTime",
+                float(self.get_parameter("exposure_time_us").value),
+            )
+            self._set_required_enum("GainAuto", "Off")
+            self._set_optional_float(
+                "Gain", float(self.get_parameter("gain_db").value)
+            )
+
+        self._require_ok(
+            "set latest-frame grab strategy",
+            camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
+        )
+        self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
+        self._grabbing = True
+        self.get_logger().info(
+            "Opened Hikrobot camera "
+            f"model={selected.model} serial={selected.serial} "
+            f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
+            "encoding=mono8"
+        )
+
+    def _set_required_enum(self, name: str, value: str) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetEnumValueByString(name, value),
+        )
+
+    def _set_required_integer(self, name: str, value: int) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetIntValueEx(name, value),
+        )
+
+    def _set_optional_bool(self, name: str, value: bool) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetBoolValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    def _set_optional_float(self, name: str, value: float) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetFloatValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    @staticmethod
+    def _require_ok(operation: str, result: int) -> None:
+        if result != 0:
+            hint = ""
+            if result == 0x80000203:
+                hint = (
+                    "; access denied: stop acquisition and close the MVS "
+                    "viewer or any other process using this camera"
+                )
+            raise RuntimeError(
+                f"MVS failed to {operation}: 0x{result:08x}{hint}"
+            )
+
+    def _grab_loop(self) -> None:
+        assert self._camera is not None
+        assert self._mvs is not None
+        timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
+        while not self._stop_event.is_set():
+            frame = self._mvs.MV_FRAME_OUT()
+            memset(byref(frame), 0, sizeof(frame))
+            result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
+            if result != 0:
+                self._timeout_count += 1
+                if self._timeout_count == 1 or self._timeout_count % 10 == 0:
+                    self.get_logger().warning(
+                        "MVS image acquisition timed out or failed: "
+                        f"0x{result:08x}; consecutive={self._timeout_count}"
+                    )
+                continue
+            try:
+                self._timeout_count = 0
+                self._publish_frame(frame)
+            except Exception as error:
+                self.get_logger().error(f"Failed to publish MVS frame: {error}")
+            finally:
+                free_result = self._camera.MV_CC_FreeImageBuffer(frame)
+                if free_result != 0:
+                    self.get_logger().error(
+                        f"MVS failed to release frame: 0x{free_result:08x}"
+                    )
+
+    def _publish_frame(self, frame: Any) -> None:
+        assert self._mvs is not None
+        info = frame.stFrameInfo
+        width = int(info.nWidth)
+        height = int(info.nHeight)
+        if width != self.image_width or height != self.image_height:
+            raise RuntimeError(
+                f"received {width}x{height}, expected "
+                f"{self.image_width}x{self.image_height}"
+            )
+        if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
+            raise RuntimeError(
+                f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
+            )
+        expected_size = width * height
+        if int(info.nFrameLen) < expected_size:
+            raise RuntimeError(
+                f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
+            )
+        stamp = self.get_clock().now().to_msg()
+        image = Image()
+        image.header.stamp = stamp
+        image.header.frame_id = self.frame_id
+        image.height = height
+        image.width = width
+        image.encoding = "mono8"
+        image.is_bigendian = 0
+        image.step = width
+        image.data = string_at(frame.pBufAddr, expected_size)
+
+        camera_info = self._make_camera_info(width, height)
+        camera_info.header.stamp = stamp
+        camera_info.header.frame_id = self.frame_id
+        self.image_publisher.publish(image)
+        self.camera_info_publisher.publish(camera_info)
+        self._frame_count += 1
+
+    def _make_camera_info(self, width: int, height: int) -> CameraInfo:
+        message = CameraInfo()
+        message.width = width
+        message.height = height
+        with self._camera_info_lock:
+            calibration = self._calibration
+        if calibration is None:
+            message.distortion_model = "plumb_bob"
+            message.d = []
+            message.k = [0.0] * 9
+            message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+            message.p = [0.0] * 12
+            return message
+        message.distortion_model = calibration.distortion_model
+        message.d = list(calibration.d)
+        message.k = list(calibration.k)
+        message.r = list(calibration.r)
+        message.p = list(calibration.p)
+        return message
+
+    def _set_camera_info_callback(
+        self,
+        request: SetCameraInfo.Request,
+        response: SetCameraInfo.Response,
+    ) -> SetCameraInfo.Response:
+        try:
+            if self.camera_info_path is None:
+                raise ValueError(
+                    "camera_info_url is empty; set it before saving calibration"
+                )
+            message = request.camera_info
+            if (
+                int(message.width) != self.image_width
+                or int(message.height) != self.image_height
+            ):
+                raise ValueError(
+                    f"calibration is {message.width}x{message.height}, expected "
+                    f"{self.image_width}x{self.image_height}"
+                )
+            mapping = calibration_to_mapping(self.camera_name, message)
+            self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
+            temporary_path = self.camera_info_path.with_suffix(
+                self.camera_info_path.suffix + ".tmp"
+            )
+            temporary_path.write_text(
+                yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
+            )
+            temporary_path.replace(self.camera_info_path)
+            calibration = load_camera_calibration(self.camera_info_path)
+            self._validate_calibration_dimensions(calibration)
+            with self._camera_info_lock:
+                self._calibration = calibration
+            response.success = True
+            response.status_message = (
+                f"saved camera calibration to {self.camera_info_path}"
+            )
+            self.get_logger().info(response.status_message)
+        except Exception as error:
+            response.success = False
+            response.status_message = str(error)
+            self.get_logger().error(
+                f"Rejected camera calibration: {response.status_message}"
+            )
+        return response
+
+    def close(self) -> None:
+        if self._closed:
+            return
+        self._closed = True
+        self._stop_event.set()
+        if self._grab_thread is not None:
+            self._grab_thread.join(timeout=2.0)
+        if self._camera is not None and self._camera_opened:
+            if self._grabbing:
+                result = self._camera.MV_CC_StopGrabbing()
+                if result != 0:
+                    self.get_logger().warning(
+                        f"MVS failed to stop grabbing: 0x{result:08x}"
+                    )
+                self._grabbing = False
+            result = self._camera.MV_CC_CloseDevice()
+            if result != 0:
+                self.get_logger().warning(
+                    f"MVS failed to close camera: 0x{result:08x}"
+                )
+            self._camera.MV_CC_DestroyHandle()
+        elif self._camera is not None and self._handle_created:
+            self._camera.MV_CC_DestroyHandle()
+        self._camera = None
+        self._handle_created = False
+        self._camera_opened = False
+        if self._sdk_initialized and self._mvs is not None:
+            self._mvs.MvCamera.MV_CC_Finalize()
+            self._sdk_initialized = False
+
+    def destroy_node(self) -> None:
+        self.close()
+        super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: HikrobotCameraNode | None = None
+    try:
+        node = HikrobotCameraNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py

@@ -1,4 +1,4 @@
-"""ROS 2 hardware calibration node for a front-facing RealSense camera."""
+"""ROS 2 hardware calibration node for a front-facing industrial camera."""
 
 from __future__ import annotations
 

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -0,0 +1,860 @@
+"""Safely replay a complete three-camera session without moving the hand."""
+
+from __future__ import annotations
+
+import argparse
+from collections import defaultdict
+from dataclasses import replace
+import hashlib
+import json
+import math
+import os
+from pathlib import Path
+import re
+import tempfile
+from typing import Any, Mapping, Sequence
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+import yaml
+
+from .extrinsics import load_three_camera_extrinsics
+from .full_hand import (
+    HandCalibrationProfile,
+    JointCurveFit,
+    build_calibration_motion_command,
+    build_compact_payload,
+    get_hand_calibration_profile,
+    validate_compact_payload,
+)
+from .storage import atomic_write_json
+from .urdf_zero import (
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    fit_joint_axis_measurement,
+    fit_rotation_joint_curve,
+    get_zero_calibration_profile,
+    rotation_curve_holdout_errors,
+    solve_urdf_zero_offsets,
+    write_zero_corrected_urdf,
+)
+
+
+def _sha256(path: Path) -> str:
+    digest = hashlib.sha256()
+    with path.open("rb") as stream:
+        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _output_suffix(output_tag: str | None) -> str:
+    """Return a filename-safe suffix for a non-destructive replay variant."""
+    if output_tag is None:
+        return ""
+    tag = str(output_tag)
+    if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
+        raise ValueError(
+            "output tag must be 1-64 filename-safe characters, beginning "
+            "with a letter or digit"
+        )
+    return f"_{tag}"
+
+
+def _load_parameters(path: Path) -> dict[str, Any]:
+    payload = yaml.safe_load(path.read_text(encoding="utf-8"))
+    if not isinstance(payload, Mapping):
+        raise ValueError("calibration config must contain a mapping")
+    node = payload.get("/**", payload.get("g20_calibration"))
+    if not isinstance(node, Mapping) or not isinstance(
+        node.get("ros__parameters"), Mapping
+    ):
+        raise ValueError("calibration config is missing ros__parameters")
+    return dict(node["ros__parameters"])
+
+
+def _latest_attempt_records(
+    rows: Sequence[Mapping[str, Any]],
+) -> dict[str, list[dict[str, Any]]]:
+    """Reproduce the online retry buffer from append-only raw samples.
+
+    Online retry clears only the failed joint/cycle/direction from memory,
+    while JSONL deliberately retains every attempt for audit.  Offline replay
+    must therefore select the greatest attempt independently for each logical
+    trajectory rather than mixing rejected attempts into the final fit.
+    """
+    samples = [dict(row) for row in rows if row.get("kind") == "sample"]
+    latest_attempt: dict[tuple[str, int, str], int] = {}
+    for row in samples:
+        key = (
+            str(row["joint"]),
+            int(row["cycle"]),
+            str(row["direction"]),
+        )
+        latest_attempt[key] = max(
+            latest_attempt.get(key, 0), int(row.get("attempt", 1))
+        )
+    records: dict[str, list[dict[str, Any]]] = defaultdict(list)
+    for row in samples:
+        key = (
+            str(row["joint"]),
+            int(row["cycle"]),
+            str(row["direction"]),
+        )
+        if int(row.get("attempt", 1)) == latest_attempt[key]:
+            records[key[0]].append(row)
+    return dict(records)
+
+
+def _load_raw_session(
+    session_dir: Path,
+) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
+    raw_path = session_dir / "raw_samples.jsonl"
+    if not raw_path.is_file():
+        raise ValueError(f"raw session does not exist: {raw_path}")
+    rows = [
+        json.loads(line)
+        for line in raw_path.read_text(encoding="utf-8").splitlines()
+        if line.strip()
+    ]
+    starts = [row for row in rows if row.get("kind") == "session_start"]
+    if len(starts) != 1:
+        raise ValueError("raw session must contain exactly one session_start")
+    return starts[0], _latest_attempt_records(rows), raw_path
+
+
+def _fit_curve(
+    name: str,
+    records: Sequence[Mapping[str, Any]],
+    *,
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+) -> JointCurveFit:
+    motor = profile.joint_specs[name].motor_index
+    return fit_rotation_joint_curve(
+        records,
+        zero_command_u8=int(baseline[motor]),
+        command_increasing=profile.joint_specs[name].command_increasing,
+    )
+
+
+def _fit_axes(
+    *,
+    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+    extrinsics_file: Path,
+    repetitions: int,
+) -> list[JointAxisMeasurement]:
+    zero_profile = get_zero_calibration_profile(profile.side, profile.model)
+    extrinsics = load_three_camera_extrinsics(extrinsics_file)
+    cache: dict[tuple[str, int], JointAxisMeasurement] = {}
+    upstream_by_joint = zero_profile.parallel_axis_parent_joint
+
+    def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
+        key = (name, cycle)
+        if key in cache:
+            return cache[key]
+        upstream = upstream_by_joint.get(name)
+        constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
+        spec = profile.joint_specs[name]
+        view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
+            [0.0, 0.0, 1.0], dtype=float
+        )
+        result = fit_joint_axis_measurement(
+            name,
+            records_by_joint[name],
+            cycle=cycle,
+            zero_command_u8=int(baseline[spec.motor_index]),
+            axis_common_constraint=constraint,
+            constrained_circle_joints=zero_profile.constrained_circle_joints,
+            view_normal_common_xyz=view_normal,
+            command_increasing=spec.command_increasing,
+        )
+        condition = build_calibration_motion_command(
+            spec,
+            int(baseline[spec.motor_index]),
+            baseline=baseline,
+            profile=profile,
+        )
+        result = replace(
+            result,
+            condition_command_u8=tuple(float(value) for value in condition),
+            view_normal_common_xyz=tuple(float(value) for value in view_normal),
+        )
+        cache[key] = result
+        return result
+
+    return [
+        fit_one(name, cycle)
+        for name in zero_profile.axis_joints
+        for cycle in range(repetitions)
+    ]
+
+
+def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
+    maximum = 0.0
+    for left in axes:
+        for right in axes:
+            maximum = max(
+                maximum,
+                math.acos(
+                    abs(
+                        float(
+                            np.clip(
+                                np.asarray(left, dtype=float)
+                                @ np.asarray(right, dtype=float),
+                                -1.0,
+                                1.0,
+                            )
+                        )
+                    )
+                ),
+            )
+    return maximum
+
+
+def _quality_failures(
+    *,
+    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+    fits: Mapping[str, JointCurveFit],
+    axes: Sequence[JointAxisMeasurement],
+    parameters: Mapping[str, Any],
+) -> list[str]:
+    failures: list[str] = []
+    repetitions = int(parameters["repetitions"])
+    zero_profile = get_zero_calibration_profile(profile.side, profile.model)
+    axis_by_key = {(item.joint, item.cycle): item for item in axes}
+    expected_directions = {"decreasing", "increasing"}
+    for name in profile.measured_joints:
+        records = list(records_by_joint.get(name, ()))
+        if not records:
+            failures.append(f"{name}: no samples")
+            continue
+        attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
+        for record in records:
+            attempts_by_direction[
+                (int(record["cycle"]), str(record["direction"]))
+            ].add(int(record.get("attempt", 1)))
+        for cycle in range(repetitions):
+            for direction in expected_directions:
+                selected = [
+                    record
+                    for record in records
+                    if int(record["cycle"]) == cycle
+                    and str(record["direction"]) == direction
+                ]
+                attempts = attempts_by_direction.get((cycle, direction), set())
+                if len(attempts) != 1:
+                    attempt_list = sorted(attempts)
+                    failures.append(
+                        f"{name} cycle {cycle + 1} {direction}: "
+                        f"ambiguous attempts {attempt_list}"
+                    )
+                    continue
+                commands = sorted({int(record["command_u8"]) for record in selected})
+                if len(selected) < int(parameters["minimum_sweep_frames"]):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
+                if not commands or max(commands) - min(commands) < float(
+                    parameters["minimum_state_span_u8"]
+                ):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
+                if len(commands) < int(parameters["minimum_sweep_bins"]):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
+                if 0 not in commands or 255 not in commands:
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
+                if commands and max(np.diff(commands), default=0) > int(
+                    parameters["maximum_bin_gap"]
+                ):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
+
+        sync_p95 = float(
+            np.percentile(
+                [float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
+                95.0,
+            )
+        )
+        if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
+            failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
+        spec = profile.joint_specs[name]
+        fit = fits[name]
+        orthogonal_limit = math.radians(
+            float(
+                parameters[
+                    "active_maximum_rotation_orthogonal_rms_deg"
+                    if spec.active
+                    else "passive_maximum_rotation_orthogonal_rms_deg"
+                ]
+            )
+        )
+        if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
+            failures.append(f"{name}: rotation orthogonal RMS")
+        if float(fit.quality["arc_rad"]) < math.radians(
+            float(parameters["trajectory_minimum_arc_deg"])
+        ):
+            failures.append(f"{name}: trajectory arc")
+        monotonic_limit = math.radians(
+            float(
+                parameters[
+                    "maximum_monotonic_correction_deg"
+                    if spec.active
+                    else "passive_maximum_monotonic_correction_deg"
+                ]
+            )
+        )
+        hysteresis_limit = math.radians(
+            float(
+                parameters[
+                    "maximum_hysteresis_deg"
+                    if spec.active
+                    else "passive_maximum_hysteresis_deg"
+                ]
+            )
+        )
+        if fit.maximum_monotonic_correction_rad > monotonic_limit:
+            failures.append(f"{name}: monotonic correction")
+        if fit.maximum_hysteresis_rad > hysteresis_limit:
+            failures.append(f"{name}: hysteresis")
+
+        cycle_travels: list[float] = []
+        cycle_axes: list[Sequence[float]] = []
+        cycle_axis_sources: list[str] = []
+        for cycle in range(repetitions):
+            cycle_fit = _fit_curve(
+                name,
+                [record for record in records if int(record["cycle"]) == cycle],
+                profile=profile,
+                baseline=baseline,
+            )
+            cycle_travels.append(
+                abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
+            )
+            axis = axis_by_key[(name, cycle)]
+            cycle_axes.append(axis.axis_common_xyz)
+            cycle_axis_sources.append(axis.axis_direction_source)
+            if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
+                failures.append(f"{name} cycle {cycle + 1}: radial RMS")
+            if axis.pose_axis_line_rms_m > float(
+                parameters["axis_maximum_pose_line_rms_m"]
+            ):
+                failures.append(
+                    f"{name} cycle {cycle + 1}: pose axis-line RMS"
+                )
+            if name not in zero_profile.constrained_circle_joints:
+                plane_limit = float(
+                    parameters[
+                        "axis_maximum_plane_rms_m"
+                        if spec.active
+                        else "passive_axis_maximum_plane_rms_m"
+                    ]
+                )
+                if axis.plane_rms_m > plane_limit:
+                    failures.append(f"{name} cycle {cycle + 1}: plane RMS")
+                if (
+                    name not in zero_profile.constrained_circle_joints
+                    and axis.rotation_circle_axis_difference_rad > math.radians(
+                        float(parameters["axis_maximum_rotation_circle_difference_deg"])
+                    )
+                ):
+                    failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
+        travel_limit = math.radians(
+            float(
+                parameters[
+                    "trajectory_maximum_cycle_travel_difference_deg"
+                    if spec.active
+                    else "passive_maximum_cycle_travel_difference_deg"
+                ]
+            )
+        )
+        if max(cycle_travels) - min(cycle_travels) > travel_limit:
+            failures.append(f"{name}: cycle travel difference")
+        if (
+            not all(
+                source == "upstream_constraint"
+                for source in cycle_axis_sources
+            )
+            and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
+                float(parameters["zero_maximum_axis_cycle_difference_deg"])
+            )
+        ):
+            failures.append(f"{name}: cycle axis difference")
+    return failures
+
+
+def _joint_xml(path: Path) -> dict[str, ET.Element]:
+    return {
+        str(joint.get("name")): joint
+        for joint in ET.parse(path).getroot().findall("joint")
+    }
+
+
+def _triplet(value: str) -> np.ndarray:
+    return np.asarray([float(item) for item in value.split()], dtype=float)
+
+
+def _validate_corrected_urdf(
+    *,
+    source: Path,
+    corrected: Path,
+    offsets: Mapping[str, float],
+    axes: Sequence[JointAxisMeasurement],
+    curves: Mapping[str, JointCurveFit],
+    motor_by_joint: Mapping[str, int],
+    inherited_zero_joints: Mapping[str, str],
+) -> dict[str, float]:
+    source_joints = _joint_xml(source)
+    corrected_joints = _joint_xml(corrected)
+    if set(source_joints) != set(corrected_joints):
+        raise ValueError("corrected URDF changed the joint set")
+    maximum_origin_rotation_error = 0.0
+    maximum_origin_translation_error = 0.0
+    for name, original_joint in source_joints.items():
+        corrected_joint = corrected_joints[name]
+        original_origin = original_joint.find("origin")
+        corrected_origin = corrected_joint.find("origin")
+        if original_origin is None or corrected_origin is None:
+            continue
+        original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
+        corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
+        maximum_origin_translation_error = max(
+            maximum_origin_translation_error,
+            float(np.linalg.norm(corrected_xyz - original_xyz)),
+        )
+        original_rotation = Rotation.from_euler(
+            "xyz", _triplet(original_origin.get("rpy", "0 0 0"))
+        )
+        corrected_rotation = Rotation.from_euler(
+            "xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
+        )
+        expected = original_rotation
+        if name in offsets:
+            axis_node = original_joint.find("axis")
+            axis = _triplet(
+                "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
+            )
+            axis /= np.linalg.norm(axis)
+            expected = original_rotation * Rotation.from_rotvec(
+                axis * float(offsets[name])
+            )
+        error = float((expected.inv() * corrected_rotation).magnitude())
+        maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
+        original_limit = original_joint.find("limit")
+        corrected_limit = corrected_joint.find("limit")
+        if original_limit is not None and corrected_limit is not None:
+            if (
+                original_limit.get("lower") != corrected_limit.get("lower")
+                or original_limit.get("upper") != corrected_limit.get("upper")
+            ):
+                raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
+    if maximum_origin_translation_error > 1.0e-12:
+        raise ValueError("corrected URDF changed a joint origin translation")
+    if maximum_origin_rotation_error > 1.0e-10:
+        raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
+    original_model = UrdfKinematicModel(source)
+    corrected_model = UrdfKinematicModel(corrected)
+    maximum_axis_error = 0.0
+    maximum_point_error = 0.0
+    for measurement in axes:
+        state = (
+            measurement.condition_state_u8
+            if measurement.condition_command_u8 is None
+            else measurement.condition_command_u8
+        )
+        angles = _angles_from_state(
+            state,
+            curves=curves,
+            motor_by_joint=motor_by_joint,
+            inherited_zero_joints=inherited_zero_joints,
+        )
+        expected_axis, expected_point = original_model.axis_line(
+            measurement.joint,
+            zero_offsets=offsets,
+            joint_angles=angles,
+        )
+        actual_axis, actual_point = corrected_model.axis_line(
+            measurement.joint,
+            zero_offsets={},
+            joint_angles=angles,
+        )
+        maximum_axis_error = max(
+            maximum_axis_error,
+            math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
+        )
+        maximum_point_error = max(
+            maximum_point_error,
+            float(np.linalg.norm(expected_point - actual_point)),
+        )
+    if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
+        raise ValueError("written URDF kinematics differ from the solved correction")
+    return {
+        "maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
+        "maximum_origin_translation_error_m": maximum_origin_translation_error,
+        "maximum_axis_equivalence_error_rad": maximum_axis_error,
+        "maximum_axis_point_equivalence_error_m": maximum_point_error,
+    }
+
+
+def replay_session(
+    session_dir: str | Path,
+    *,
+    serial_number: str | None = None,
+    config_file: str | Path | None = None,
+    write_outputs: bool = False,
+    output_tag: str | None = None,
+) -> dict[str, Any]:
+    session = Path(session_dir).expanduser().resolve()
+    package_root = Path(__file__).resolve().parents[1]
+    config = (
+        package_root / "config" / "three_camera_calibration.yaml"
+        if config_file is None
+        else Path(config_file).expanduser().resolve()
+    )
+    parameters = _load_parameters(config)
+    start, records_by_joint, raw_path = _load_raw_session(session)
+    hand_model = str(start.get("hand_model", "G20")).upper()
+    side = str(start["hand_type"]).lower()
+    profile = get_hand_calibration_profile(side, hand_model)
+    zero_profile = get_zero_calibration_profile(side, hand_model)
+    baseline = tuple(int(value) for value in start["baseline_command_u8"])
+    if len(baseline) != 20:
+        raise ValueError("session baseline must contain exactly 20 commands")
+    if set(records_by_joint) != set(profile.measured_joints):
+        raise ValueError("raw session does not contain exactly the measured joint set")
+    source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
+    extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
+    if not source_urdf.is_file() or not extrinsics_file.is_file():
+        raise ValueError("session source URDF or camera extrinsics is missing")
+    if "zero_calibrated" in source_urdf.stem.lower():
+        raise ValueError("offline replay requires the original CAD URDF")
+    hand_serial = str(serial_number or session.parent.name)
+    output_suffix = _output_suffix(output_tag)
+    repetitions = int(parameters["repetitions"])
+
+    measured_fits = {
+        name: _fit_curve(
+            name,
+            records_by_joint[name],
+            profile=profile,
+            baseline=baseline,
+        )
+        for name in profile.measured_joints
+    }
+    training_fits = {
+        name: _fit_curve(
+            name,
+            [
+                record
+                for record in records_by_joint[name]
+                if int(record["cycle"]) in {0, 1}
+            ],
+            profile=profile,
+            baseline=baseline,
+        )
+        for name in profile.measured_joints
+    }
+    axes = _fit_axes(
+        records_by_joint=records_by_joint,
+        profile=profile,
+        baseline=baseline,
+        extrinsics_file=extrinsics_file,
+        repetitions=repetitions,
+    )
+    failures = _quality_failures(
+        records_by_joint=records_by_joint,
+        profile=profile,
+        baseline=baseline,
+        fits=measured_fits,
+        axes=axes,
+        parameters=parameters,
+    )
+    if failures:
+        raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
+
+    holdout_by_joint = {
+        name: rotation_curve_holdout_errors(
+            training_fits[name],
+            [
+                record
+                for record in records_by_joint[name]
+                if int(record["cycle"]) == 2
+            ],
+            zero_command_u8=int(
+                baseline[profile.joint_specs[name].motor_index]
+            ),
+        )
+        for name in profile.measured_joints
+    }
+    trajectory_errors = np.abs(
+        np.asarray(
+            [value for values in holdout_by_joint.values() for value in values],
+            dtype=float,
+        )
+    )
+    maximum_validation_mae = math.radians(
+        float(parameters["maximum_validation_mae_deg"])
+    )
+    maximum_validation_p95 = math.radians(
+        float(parameters["maximum_validation_p95_deg"])
+    )
+    trajectory_mae = float(np.mean(trajectory_errors))
+    trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
+    if (
+        trajectory_mae > maximum_validation_mae
+        or trajectory_p95 > maximum_validation_p95
+    ):
+        raise ValueError("third-cycle trajectory holdout failed")
+
+    motor_by_joint = {
+        name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
+    }
+    joint_limits: dict[str, float] = {}
+    solve_arguments = {
+        "source_urdf": source_urdf,
+        "measurements": axes,
+        "motor_by_joint": motor_by_joint,
+        "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
+        "finger_maximum_offset_rad": math.radians(
+            float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
+        ),
+        "joint_maximum_offset_rad": joint_limits,
+        "maximum_cycle_difference_rad": math.radians(
+            float(parameters["zero_maximum_axis_cycle_difference_deg"])
+        ),
+        "maximum_axis_cone_mismatch_rad": math.radians(
+            float(parameters["zero_maximum_axis_cone_mismatch_deg"])
+        ),
+        "maximum_pose_axis_line_rms_m": float(
+            parameters["axis_maximum_pose_line_rms_m"]
+        ),
+        "maximum_validation_mae_rad": maximum_validation_mae,
+        "maximum_validation_p95_rad": maximum_validation_p95,
+        "hand_type": side,
+        "hand_model": hand_model,
+    }
+    holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
+    if not holdout_zero.passed:
+        failure = {
+            "reasons": dict(holdout_zero.failure_reasons),
+            "fitted_offsets_deg": {
+                name: math.degrees(value)
+                for name, value in holdout_zero.direct_offsets_rad.items()
+            },
+            "cycle_offsets_deg": {
+                name: [math.degrees(value) for value in values]
+                for name, values in holdout_zero.cycle_offsets_rad.items()
+            },
+        }
+        raise ValueError(
+            "third-cycle zero/URDF holdout failed: "
+            + json.dumps(failure, ensure_ascii=False, sort_keys=True)
+        )
+    final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
+    if not final_zero.passed:
+        raise ValueError(
+            "all-cycle zero refit failed: "
+            + json.dumps(
+                {
+                    "reasons": dict(final_zero.failure_reasons),
+                    "fitted_offsets_deg": {
+                        name: math.degrees(value)
+                        for name, value in final_zero.direct_offsets_rad.items()
+                    },
+                },
+                ensure_ascii=False,
+                sort_keys=True,
+            )
+        )
+    for target, source_name in zero_profile.inherited_static_zero_joints.items():
+        if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
+            raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
+    for target in (
+        set(zero_profile.inherited_zero_joints)
+        - set(zero_profile.inherited_static_zero_joints)
+    ):
+        if final_zero.all_active_offsets_rad[target] != 0.0:
+            raise ValueError(f"unobserved static zero must retain source CAD: {target}")
+
+    validation_errors = [
+        float(value) for values in holdout_by_joint.values() for value in values
+    ]
+    validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
+    payload = build_compact_payload(
+        serial_number=hand_serial,
+        measured_fits=measured_fits,
+        urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
+        validation_errors_rad=validation_errors,
+        passed=True,
+        baseline=baseline,
+        side=side,
+        model=hand_model,
+    )
+    validate_compact_payload(payload)
+
+    stamp = session.name
+    final_json = session / (
+        f"{hand_model.lower()}_{side}_{hand_serial}_calibration"
+        f"{output_suffix}.json"
+    )
+    expected_urdf_name = (
+        f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
+        f"{output_suffix}.urdf"
+    )
+    final_urdf = source_urdf.parent / expected_urdf_name
+    report_path = session / (
+        f"{hand_model.lower()}_{side}_{hand_serial}_offline_validation"
+        f"{output_suffix}.json"
+    )
+    if write_outputs:
+        existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
+        if existing:
+            raise ValueError(
+                "refusing to overwrite replay outputs: "
+                + ", ".join(str(path) for path in existing)
+            )
+
+    source_hash_before = _sha256(source_urdf)
+    with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
+        candidate = write_zero_corrected_urdf(
+            source_urdf=source_urdf,
+            output_directory=temporary,
+            serial_number=hand_serial,
+            offsets_rad=final_zero.all_active_offsets_rad,
+            timestamp=stamp,
+        )
+        urdf_checks = _validate_corrected_urdf(
+            source=source_urdf,
+            corrected=candidate,
+            offsets=final_zero.all_active_offsets_rad,
+            axes=axes,
+            curves=measured_fits,
+            motor_by_joint=motor_by_joint,
+            inherited_zero_joints=zero_profile.inherited_zero_joints,
+        )
+        residual_zero = solve_urdf_zero_offsets(
+            curves=measured_fits,
+            fixed_direct_zero_offsets_rad={
+                name: 0.0
+                for name in zero_profile.fixed_direct_zero_offsets_rad
+            },
+            static_output_zero_offsets_rad={
+                name: 0.0
+                for name in zero_profile.static_output_zero_offsets_rad
+            },
+            **{**solve_arguments, "source_urdf": candidate},
+        )
+        maximum_residual_offset = max(
+            abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
+        )
+        if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
+            raise ValueError("written URDF retains a significant zero correction")
+        candidate_hash = _sha256(candidate)
+        if write_outputs:
+            os.replace(candidate, final_urdf)
+
+    if _sha256(source_urdf) != source_hash_before:
+        raise ValueError("source URDF changed during offline replay")
+
+    report: dict[str, Any] = {
+        "passed": True,
+        "session_dir": str(session),
+        "model": hand_model,
+        "side": side,
+        "serial_number": hand_serial,
+        "output_tag": output_tag,
+        "raw_samples_sha256": _sha256(raw_path),
+        "source_urdf": str(source_urdf),
+        "source_urdf_sha256": source_hash_before,
+        "joint_limits_deg": {
+            "finger_default": float(
+                parameters.get("zero_finger_maximum_offset_deg", 3.0)
+            ),
+            "thumb_default": float(parameters["zero_maximum_offset_deg"]),
+        },
+        "static_zero_policy": "direct_measurements_only",
+        "fixed_zero_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
+        },
+        "static_output_zero_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in zero_profile.static_output_zero_offsets_rad.items()
+        },
+        "direct_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in final_zero.direct_offsets_rad.items()
+        },
+        "cycle_offsets_deg": {
+            name: [math.degrees(value) for value in values]
+            for name, values in holdout_zero.cycle_offsets_rad.items()
+        },
+        "offset_uncertainty_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.offset_uncertainty_rad.items()
+        },
+        "trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
+        "trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
+        "zero_holdout_error_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.validation_error_by_joint_rad.items()
+        },
+        "zero_original_error_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
+        },
+        "zero_improvement_95pct_lower_deg": {
+            name: math.degrees(value)
+            for name, value in (
+                holdout_zero.validation_improvement_confidence_lower_rad.items()
+            )
+        },
+        "corrected_urdf_checks": {
+            **urdf_checks,
+            "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
+        },
+        "corrected_urdf_sha256": candidate_hash,
+        "final_json": str(final_json) if write_outputs else None,
+        "corrected_urdf": str(final_urdf) if write_outputs else None,
+    }
+    if write_outputs:
+        atomic_write_json(final_json, payload)
+        report["final_json_sha256"] = _sha256(final_json)
+        if _sha256(final_urdf) != candidate_hash:
+            raise ValueError("formal corrected URDF differs from validated candidate")
+        atomic_write_json(report_path, report)
+        report["validation_report"] = str(report_path)
+    return report
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description=(
+            "Replay and independently validate a complete supported-hand "
+            "calibration session."
+        )
+    )
+    parser.add_argument("session_dir")
+    parser.add_argument("--serial-number", default=None)
+    parser.add_argument("--config-file", default=None)
+    parser.add_argument("--write", action="store_true")
+    parser.add_argument(
+        "--output-tag",
+        default=None,
+        help="safe suffix for a replay variant; existing outputs are never overwritten",
+    )
+    arguments = parser.parse_args()
+    result = replay_session(
+        arguments.session_dir,
+        serial_number=arguments.serial_number,
+        config_file=arguments.config_file,
+        write_outputs=arguments.write,
+        output_tag=arguments.output_tag,
+    )
+    print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py

@@ -41,6 +41,19 @@ def _relative_pose(
     return relative_rotation, relative_translation
 
 
+def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float:
+    """Return the undirected angle between two observed Tag face normals."""
+    first_normal = Rotation.from_quat(first.quaternion_xyzw).apply(
+        [0.0, 0.0, 1.0]
+    )
+    second_normal = Rotation.from_quat(second.quaternion_xyzw).apply(
+        [0.0, 0.0, 1.0]
+    )
+    return math.acos(
+        abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0)))
+    )
+
+
 def select_rigid_group_trajectory(
     frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
     *,
@@ -50,6 +63,8 @@ def select_rigid_group_trajectory(
     rotation_scale_rad: float,
     translation_scale_m: float,
     pair_geometry: str = "pose",
+    normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+    normal_alignment_scale_rad: float | None = None,
 ) -> tuple[
     list[dict[str, SquareTagPose]],
     dict[str, float | str],
@@ -66,20 +81,34 @@ def select_rigid_group_trajectory(
     """
     role_names = tuple(str(role) for role in roles)
     pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    normal_pair_names = tuple(
+        (str(first), str(second))
+        for first, second in normal_alignment_pairs
+    )
     if not frames:
         raise ValueError("at least one PnP frame is required")
     if len(set(role_names)) != len(role_names) or not role_names:
         raise ValueError("roles must be non-empty and unique")
     if any(
         parent not in role_names or child not in role_names
-        for parent, child in pair_names
+        for parent, child in (*pair_names, *normal_pair_names)
     ):
-        raise ValueError("fixed_pairs must reference roles")
+        raise ValueError("geometry pairs must reference roles")
     reprojection_scale = float(reprojection_scale_px)
     rotation_scale = float(rotation_scale_rad)
     translation_scale = float(translation_scale_m)
     geometry_mode = str(pair_geometry)
-    if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0:
+    normal_scale = (
+        rotation_scale
+        if normal_alignment_scale_rad is None
+        else float(normal_alignment_scale_rad)
+    )
+    if min(
+        reprojection_scale,
+        rotation_scale,
+        translation_scale,
+        normal_scale,
+    ) <= 0.0:
         raise ValueError("trajectory selection scales must be positive")
     if geometry_mode not in {"pose", "distance"}:
         raise ValueError("pair_geometry must be pose or distance")
@@ -110,6 +139,12 @@ def select_rigid_group_trajectory(
             pose.reprojection_error_px
             for pose in combination.values()
         ) / reprojection_scale
+        normal_alignment_cost = sum(
+            _normal_alignment_rad(
+                combination[first], combination[second]
+            )
+            for first, second in normal_pair_names
+        ) / normal_scale
         rotation_drifts: list[float] = []
         translation_drifts: list[float] = []
         distance_drifts: list[float] = []
@@ -156,7 +191,7 @@ def select_rigid_group_trajectory(
                 + sum(translation_drifts) / translation_scale
             )
         return (
-            reprojection_cost + geometry_cost,
+            reprojection_cost + geometry_cost + normal_alignment_cost,
             max(rotation_drifts, default=0.0),
             (
                 max(distance_drifts, default=0.0)
@@ -384,9 +419,25 @@ def select_rigid_group_trajectory(
         translation_drifts_by_frame, dtype=float
     )
     distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float)
+    normal_alignments = np.asarray(
+        [
+            max(
+                (
+                    _normal_alignment_rad(frame[first], frame[second])
+                    for first, second in normal_pair_names
+                ),
+                default=0.0,
+            )
+            for frame in best_path
+        ],
+        dtype=float,
+    )
     return best_path, {
         "total_cost": float(best_total),
         "pair_geometry": geometry_mode,
+        "maximum_normal_alignment_rad": float(
+            np.max(normal_alignments, initial=0.0)
+        ),
         "maximum_pair_rotation_drift_rad": float(
             np.max(rotation_drifts, initial=0.0)
         ),
@@ -414,6 +465,172 @@ def select_rigid_group_trajectory(
     }
 
 
+def select_static_rigid_group_initialization(
+    frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
+    *,
+    roles: Sequence[str],
+    fixed_pairs: Sequence[tuple[str, str]],
+    reprojection_scale_px: float,
+    maximum_pose_jump_rad: float,
+    maximum_translation_jump_m: float,
+    relative_rotation_scale_rad: float,
+    relative_translation_scale_m: float,
+    normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+    normal_alignment_scale_rad: float = math.radians(5.0),
+) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
+    """Select a static multi-Tag IPPE branch path in bounded time.
+
+    Group initialization is performed while the hand is held at an endpoint,
+    so every frame should describe the same camera and relative Tag poses.
+    Enumerating a full Viterbi transition matrix for every possible first-frame
+    branch is therefore unnecessary: with four two-branch Tags and eight
+    frames it performs more than one hundred thousand scipy rotations and can
+    block the live ROS callback for several seconds.
+
+    Instead, treat every first-frame combination as a possible static
+    reference and independently select the closest combination in each later
+    frame.  This preserves the multi-frame rigidity and normal-alignment
+    evidence while changing the search from O(F*C^2*C0) to O(F*C*C0).
+    """
+    role_names = tuple(str(role) for role in roles)
+    pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    normal_pair_names = tuple(
+        (str(first), str(second)) for first, second in normal_alignment_pairs
+    )
+    if not frames:
+        raise ValueError("at least one PnP frame is required")
+    if not role_names or len(set(role_names)) != len(role_names):
+        raise ValueError("roles must be non-empty and unique")
+    if any(
+        parent not in role_names or child not in role_names
+        for parent, child in (*pair_names, *normal_pair_names)
+    ):
+        raise ValueError("geometry pairs must reference roles")
+    scales = (
+        float(reprojection_scale_px),
+        float(maximum_pose_jump_rad),
+        float(maximum_translation_jump_m),
+        float(relative_rotation_scale_rad),
+        float(relative_translation_scale_m),
+        float(normal_alignment_scale_rad),
+    )
+    if min(scales) <= 0.0:
+        raise ValueError("static initialization scales must be positive")
+    (
+        reprojection_scale,
+        pose_scale,
+        translation_scale,
+        relative_rotation_scale,
+        relative_translation_scale,
+        normal_scale,
+    ) = scales
+
+    combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
+    for frame in frames:
+        candidate_lists = [tuple(frame.get(role, ())) for role in role_names]
+        if any(not candidates for candidates in candidate_lists):
+            raise ValueError("every frame must contain every requested role")
+        combinations_by_frame.append(
+            [
+                dict(zip(role_names, combination))
+                for combination in product(*candidate_lists)
+            ]
+        )
+
+    def score_against_reference(
+        reference: Mapping[str, SquareTagPose],
+        reference_pairs: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ],
+        combination: Mapping[str, SquareTagPose],
+    ) -> float:
+        score = sum(
+            pose.reprojection_error_px for pose in combination.values()
+        ) / reprojection_scale
+        score += sum(
+            _normal_alignment_rad(combination[first], combination[second])
+            for first, second in normal_pair_names
+        ) / normal_scale
+        score += sum(
+            rotation_distance_rad(
+                reference[role].quaternion_xyzw,
+                combination[role].quaternion_xyzw,
+            )
+            / pose_scale
+            + float(
+                np.linalg.norm(
+                    np.asarray(combination[role].translation_xyz_m, dtype=float)
+                    - np.asarray(reference[role].translation_xyz_m, dtype=float)
+                )
+            )
+            / translation_scale
+            for role in role_names
+        )
+        for pair, (reference_rotation, reference_translation) in (
+            reference_pairs.items()
+        ):
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            score += (
+                float((reference_rotation.inv() * rotation).magnitude())
+                / relative_rotation_scale
+                + float(np.linalg.norm(translation - reference_translation))
+                / relative_translation_scale
+            )
+        return float(score)
+
+    best_total = float("inf")
+    best_path: list[dict[str, SquareTagPose]] | None = None
+    for reference in combinations_by_frame[0]:
+        reference_pairs = {
+            pair: _relative_pose(reference[pair[0]], reference[pair[1]])
+            for pair in pair_names
+        }
+        path = [reference]
+        total = score_against_reference(reference, reference_pairs, reference)
+        for combinations in combinations_by_frame[1:]:
+            scored = [
+                (
+                    score_against_reference(
+                        reference, reference_pairs, combination
+                    ),
+                    combination,
+                )
+                for combination in combinations
+            ]
+            cost, selected = min(scored, key=lambda item: item[0])
+            total += cost
+            path.append(selected)
+        if total < best_total:
+            best_total = total
+            best_path = path
+
+    if best_path is None:
+        raise RuntimeError("static group initialization produced no path")
+
+    # Reuse the complete quality calculation with exactly one chosen branch
+    # per role and frame.  This retains all existing quality fields without
+    # reintroducing the combinatorial branch search.
+    reduced_frames = [
+        {role: (frame[role],) for role in role_names} for frame in best_path
+    ]
+    selected_path, quality = select_rigid_group_trajectory(
+        reduced_frames,
+        roles=role_names,
+        fixed_pairs=pair_names,
+        reprojection_scale_px=reprojection_scale,
+        rotation_scale_rad=relative_rotation_scale,
+        translation_scale_m=relative_translation_scale,
+        normal_alignment_pairs=normal_pair_names,
+        normal_alignment_scale_rad=normal_scale,
+    )
+    quality = dict(quality)
+    quality["total_cost"] = float(best_total)
+    quality["initialization_search"] = "static_reference"
+    return selected_path, quality
+
+
 def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray:
     matrix = np.asarray(camera_matrix, dtype=np.float64)
     if matrix.shape != (3, 3):
@@ -517,9 +734,26 @@ def rotation_distance_rad(
     first_xyzw: Sequence[float],
     second_xyzw: Sequence[float],
 ) -> float:
-    first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float))
-    second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float))
-    return float((first.inv() * second).magnitude())
+    first = np.asarray(first_xyzw, dtype=float)
+    second = np.asarray(second_xyzw, dtype=float)
+    if first.shape != (4,) or second.shape != (4,):
+        raise ValueError("quaternions must contain four values")
+    first_norm = float(np.linalg.norm(first))
+    second_norm = float(np.linalg.norm(second))
+    if (
+        not np.all(np.isfinite(first))
+        or not np.all(np.isfinite(second))
+        or first_norm <= 0.0
+        or second_norm <= 0.0
+    ):
+        raise ValueError("quaternions must be finite and non-zero")
+    # q and -q represent the same rotation.  The absolute dot-product gives
+    # the geodesic SO(3) distance without constructing two scipy Rotation
+    # objects for every branch comparison in the live tracker.
+    cosine_half_angle = abs(
+        float(np.dot(first / first_norm, second / second_norm))
+    )
+    return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0)))
 
 
 def select_continuous_pose(
@@ -775,19 +1009,30 @@ class SquareTagGroupPoseTracker:
         reprojection_scale_px: float,
         reprojection_weight: float,
         reset_after_seconds: float,
+        initialization_frames: int = 1,
+        normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+        normal_alignment_scale_rad: float = math.radians(5.0),
+        maximum_normal_alignment_rad: float | None = None,
     ) -> None:
         self.roles = tuple(str(role) for role in roles)
         self.adjacent_pairs = tuple(
             (str(parent), str(child))
             for parent, child in adjacent_pairs
         )
+        self.normal_alignment_pairs = tuple(
+            (str(first), str(second))
+            for first, second in normal_alignment_pairs
+        )
         if not self.roles or len(set(self.roles)) != len(self.roles):
             raise ValueError("roles must be non-empty and unique")
         if any(
             parent not in self.roles or child not in self.roles
-            for parent, child in self.adjacent_pairs
+            for parent, child in (
+                *self.adjacent_pairs,
+                *self.normal_alignment_pairs,
+            )
         ):
-            raise ValueError("adjacent_pairs must reference roles")
+            raise ValueError("group geometry pairs must reference roles")
         self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
         self.maximum_translation_jump_m = float(
             maximum_translation_jump_m
@@ -800,6 +1045,15 @@ class SquareTagGroupPoseTracker:
         )
         self.reprojection_scale_px = float(reprojection_scale_px)
         self.reprojection_weight = float(reprojection_weight)
+        self.initialization_frames = int(initialization_frames)
+        self.normal_alignment_scale_rad = float(
+            normal_alignment_scale_rad
+        )
+        self.maximum_normal_alignment_rad = (
+            None
+            if maximum_normal_alignment_rad is None
+            else float(maximum_normal_alignment_rad)
+        )
         reset_seconds = float(reset_after_seconds)
         if min(
             self.maximum_pose_jump_rad,
@@ -807,19 +1061,35 @@ class SquareTagGroupPoseTracker:
             self.relative_rotation_scale_rad,
             self.relative_translation_scale_m,
             self.reprojection_scale_px,
+            self.normal_alignment_scale_rad,
             reset_seconds,
         ) <= 0.0:
             raise ValueError("group tracking scales must be positive")
         if self.reprojection_weight < 0.0:
             raise ValueError("reprojection_weight must be non-negative")
+        if self.initialization_frames < 1:
+            raise ValueError("initialization_frames must be positive")
+        if (
+            self.maximum_normal_alignment_rad is not None
+            and self.maximum_normal_alignment_rad <= 0.0
+        ):
+            raise ValueError("maximum normal alignment must be positive")
         self.reset_after_ns = int(reset_seconds * 1_000_000_000)
         self._previous: dict[str, SquareTagPose] = {}
         self._previous_stamp_ns: int | None = None
+        self._initial_candidates: list[
+            dict[str, tuple[SquareTagPose, ...]]
+        ] = []
+        self._initial_stamps_ns: list[int] = []
+        self.last_initialization_quality: dict[str, float | str] = {}
         self.branch_correction_counts: dict[str, int] = {}
 
     def reset(self) -> None:
         self._previous.clear()
         self._previous_stamp_ns = None
+        self._initial_candidates.clear()
+        self._initial_stamps_ns.clear()
+        self.last_initialization_quality.clear()
         self.branch_correction_counts.clear()
 
     def select(
@@ -856,13 +1126,110 @@ class SquareTagGroupPoseTracker:
         )
 
         if not previous_is_fresh:
-            selected = min(
-                combinations,
-                key=lambda combination: sum(
-                    pose.reprojection_error_px
-                    for pose in combination.values()
-                ),
-            )
+            if self._previous_stamp_ns is not None:
+                self._previous.clear()
+                self._previous_stamp_ns = None
+                self._initial_candidates.clear()
+                self._initial_stamps_ns.clear()
+                self.last_initialization_quality.clear()
+            if self.initialization_frames > 1:
+                if self._initial_stamps_ns and not (
+                    0 <= stamp - self._initial_stamps_ns[-1]
+                    <= self.reset_after_ns
+                ):
+                    self._initial_candidates.clear()
+                    self._initial_stamps_ns.clear()
+                self._initial_candidates.append(
+                    {
+                        role: tuple(candidates_by_role.get(role, ()))
+                        for role in self.roles
+                    }
+                )
+                self._initial_stamps_ns.append(stamp)
+                if len(self._initial_candidates) < self.initialization_frames:
+                    return (
+                        None,
+                        "group_initializing:"
+                        f"{len(self._initial_candidates)}/"
+                        f"{self.initialization_frames}",
+                    )
+                selected_path, initialization_quality = (
+                    select_static_rigid_group_initialization(
+                        self._initial_candidates,
+                        roles=self.roles,
+                        fixed_pairs=self.adjacent_pairs,
+                        reprojection_scale_px=self.reprojection_scale_px,
+                        maximum_pose_jump_rad=self.maximum_pose_jump_rad,
+                        maximum_translation_jump_m=(
+                            self.maximum_translation_jump_m
+                        ),
+                        relative_rotation_scale_rad=(
+                            self.relative_rotation_scale_rad
+                        ),
+                        relative_translation_scale_m=(
+                            self.relative_translation_scale_m
+                        ),
+                        normal_alignment_pairs=(
+                            self.normal_alignment_pairs
+                        ),
+                        normal_alignment_scale_rad=(
+                            self.normal_alignment_scale_rad
+                        ),
+                    )
+                )
+                selected = selected_path[-1]
+                stamp = self._initial_stamps_ns[-1]
+                self.last_initialization_quality = dict(
+                    initialization_quality
+                )
+                self._initial_candidates.clear()
+                self._initial_stamps_ns.clear()
+                if (
+                    self.maximum_normal_alignment_rad is not None
+                    and float(
+                        initialization_quality[
+                            "maximum_normal_alignment_rad"
+                        ]
+                    )
+                    > self.maximum_normal_alignment_rad
+                ):
+                    return None, "group_normal_alignment"
+            else:
+                selected = min(
+                    combinations,
+                    key=lambda combination: (
+                        sum(
+                            pose.reprojection_error_px
+                            for pose in combination.values()
+                        )
+                        / self.reprojection_scale_px
+                        + sum(
+                            _normal_alignment_rad(
+                                combination[first], combination[second]
+                            )
+                            for first, second in self.normal_alignment_pairs
+                        )
+                        / self.normal_alignment_scale_rad
+                    ),
+                )
+                maximum_alignment = max(
+                    (
+                        _normal_alignment_rad(
+                            selected[first], selected[second]
+                        )
+                        for first, second in self.normal_alignment_pairs
+                    ),
+                    default=0.0,
+                )
+                self.last_initialization_quality = {
+                    "maximum_normal_alignment_rad": maximum_alignment
+                }
+                if (
+                    self.maximum_normal_alignment_rad is not None
+                    and maximum_alignment
+                    > self.maximum_normal_alignment_rad
+                ):
+                    return None, "group_normal_alignment"
         else:
             previous_relative = {
                 pair: _relative_pose(

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py

@@ -30,6 +30,24 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
         os.fsync(stream.fileno())
 
 
+def append_jsonl_many(
+    path: str | Path, payloads: Iterable[Mapping[str, Any]]
+) -> None:
+    """Durably append a batch while paying the fsync cost only once."""
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    lines = [
+        json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
+        for payload in payloads
+    ]
+    if not lines:
+        return
+    with destination.open("a", encoding="utf-8") as stream:
+        stream.write("\n".join(lines) + "\n")
+        stream.flush()
+        os.fsync(stream.fileno())
+
+
 def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
     source = Path(path)
     if not source.exists():

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py

@@ -0,0 +1,504 @@
+"""Chinese, operator-facing diagnostics for three-camera calibration."""
+
+from __future__ import annotations
+
+import re
+from typing import Any, Mapping
+
+
+STATE_NAMES_ZH = {
+    "PREFLIGHT": "设备和标签预检",
+    "WAIT_START": "等待开始标定",
+    "RETURN_BASELINE": "正在恢复目标姿态",
+    "PREPARE_SWEEP": "正在到达扫描起点",
+    "SWEEP": "正在采集轨迹",
+    "FITTING": "正在拟合轨迹和零位",
+    "VALIDATION_MOVE": "正在移动到随机复测位置",
+    "VALIDATION_CAPTURE": "正在采集随机复测数据",
+    "PAUSED": "标定已暂停",
+    "ABORTED": "标定已终止",
+    "COMPLETE": "标定已完成",
+}
+
+VIEW_NAMES_ZH = {
+    "front": "正面",
+    "side": "侧面",
+    "top": "上面",
+}
+
+JOINT_NAMES_ZH = {
+    "thumb_cmc_pitch": "拇指CMC俯仰",
+    "thumb_cmc_roll": "拇指CMC滚转",
+    "thumb_mcp": "拇指MCP",
+    "thumb_ip": "拇指IP（被动）",
+    "index_mcp_roll": "食指MCP侧摆",
+    "index_mcp_pitch": "食指MCP屈伸",
+    "index_pip": "食指PIP",
+    "index_dip": "食指DIP（被动）",
+    "middle_mcp_roll": "中指MCP侧摆",
+    "middle_mcp_pitch": "中指MCP屈伸",
+    "middle_pip": "中指PIP",
+    "middle_dip": "中指DIP（被动）",
+    "ring_mcp_roll": "无名指MCP侧摆",
+    "ring_mcp_pitch": "无名指MCP屈伸",
+    "ring_pip": "无名指PIP",
+    "ring_dip": "无名指DIP（被动）",
+    "pinky_mcp_roll": "小指MCP侧摆",
+    "pinky_mcp_pitch": "小指MCP屈伸",
+    "pinky_pip": "小指PIP",
+    "pinky_dip": "小指DIP（被动）",
+    "thumb_cmc_yaw": "拇指CMC侧摆",
+}
+
+
+def _format_u8(value: Any) -> str:
+    if value is None:
+        return "尚无反馈"
+    return f"{float(value):.1f}"
+
+
+def _task_text(active: Mapping[str, Any]) -> str:
+    if not active:
+        return "尚无活动任务"
+    view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
+    if active.get("kind") == "fit_failure":
+        joints = active.get("joints", [])
+        joint_text = "/".join(
+            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+        )
+        return (
+            f"{view}机位，{joint_text}拟合检查失败，"
+            f"电机{active.get('motor_index')}，"
+            f"第{active.get('attempt', 1)}次尝试"
+        )
+    if active.get("kind") == "zero_model_failure":
+        joints = active.get("joints", [])
+        joint_text = "/".join(
+            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+        )
+        return (
+            f"{view}机位，{joint_text}零位/URDF验证失败，"
+            f"电机{active.get('motor_index')}，不会自动重扫"
+        )
+    if active.get("kind") == "motion_stall":
+        return (
+            f"电机{active.get('motor_index', '?')}运动停滞，目标"
+            f"{_format_u8(active.get('target_u8'))}、实际"
+            f"{_format_u8(active.get('actual_u8'))}"
+        )
+    if active.get("kind") == "validation":
+        return (
+            f"{view}机位，随机复测，电机{active.get('motor_index')}，"
+            f"目标命令{active.get('command_u8')}"
+        )
+    joints = active.get("joints", [])
+    joint_text = "/".join(
+        JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+    )
+    start = active.get("start_u8")
+    target = active.get("target_u8")
+    cycle = active.get("cycle", "?")
+    repetitions = active.get("repetitions", "?")
+    direction_index = active.get("direction_index")
+    direction_text = (
+        "" if direction_index is None else f"第{direction_index}/2程，"
+    )
+    task = (
+        f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
+        f"第{cycle}/{repetitions}轮，{direction_text}{start}→{target}"
+    )
+    sequence = active.get("cycle_sequence_u8", [])
+    if len(sequence) == 3:
+        task += "（本轮" + "→".join(str(value) for value in sequence) + "）"
+    fit_attempt = int(active.get("fit_attempt", 1))
+    if fit_attempt > 1:
+        task += (
+            f"（整关节自动重采第{fit_attempt}/"
+            f"{active.get('fit_attempt_limit', '?')}次）"
+        )
+    return task
+
+
+def three_camera_reason_zh(
+    state: str,
+    reason: str,
+    active: Mapping[str, Any],
+) -> tuple[str, str]:
+    """Translate a reason code and provide one concrete operator action."""
+    reason = str(reason)
+    sample = active.get("sample", {}) if active else {}
+    missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
+    sample_range = (
+        f"{_format_u8(sample.get('minimum_u8'))}～"
+        f"{_format_u8(sample.get('maximum_u8'))}"
+    )
+    tolerance = sample.get("endpoint_tolerance_u8", "?")
+
+    if reason.startswith("motor_state_stalled:"):
+        fields = reason.split(":")
+        context = fields[1] if len(fields) > 1 else "unknown"
+        error_match = re.search(r"error_u8=([0-9.]+)", reason)
+        error = error_match.group(1) if error_match else "未知"
+        motor = active.get("motor_index")
+        if motor is not None:
+            return (
+                f"电机{motor}反馈连续8秒没有向目标推进；目标"
+                f"{_format_u8(active.get('target_u8'))}、实际"
+                f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
+                f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
+                f"（阶段={context}）。程序已保持当前位置。",
+                "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
+                "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
+            )
+        return (
+            f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
+            f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
+            "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
+            "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
+        )
+
+    if "URDF zero offset reached the configured" in reason:
+        bound_match = re.search(
+            r"configured\s+([0-9.]+)\s+degree bound", reason
+        )
+        bound = bound_match.group(1) if bound_match else "配置的"
+        hit_text = ""
+        if "bound:" in reason:
+            hit_text = reason.split("bound:", 1)[1].split(
+                "; all_offsets:", 1
+            )[0]
+            for name, label in JOINT_NAMES_ZH.items():
+                hit_text = hit_text.replace(name, label)
+        hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
+        return (
+            f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
+            "零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
+            "不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
+            "内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
+            "产生的轨迹不能直接生成修正URDF。",
+        )
+
+    if reason == "sweep_missing_endpoint_bin":
+        missing_text = "、".join(str(value) for value in missing) or "0或255"
+        return (
+            f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
+            f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
+            f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
+            "端点，但该时刻没有同时取得有效Tag图像和电机状态。",
+            "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
+            "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
+        )
+    if reason == "sweep_bins_too_few":
+        return (
+            f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
+            f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
+            "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
+        )
+    if reason == "sweep_bin_gap_too_large":
+        return (
+            f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
+            f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
+            "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
+        )
+    if reason == "synchronised_tag_state_timeout":
+        return (
+            "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
+            "按时间戳配对”的图像帧。",
+            "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
+            "程序会重扫当前方向。",
+        )
+    if reason == "sweep_start_position_timeout":
+        return (
+            f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
+            f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
+            "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "sweep_timeout":
+        return (
+            "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
+            "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
+        )
+    if reason == "return_baseline_timeout":
+        return (
+            "一个或多个标定电机未在规定时间返回基准命令。",
+            "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "validation_move_timeout":
+        return (
+            "随机复测时电机未在规定时间到达目标命令。",
+            "检查机械手状态和机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "validation_capture_timeout":
+        return (
+            "随机复测位置没有采集到足够的同步有效Tag帧。",
+            "检查当前机位Tag可见性后调用resume。",
+        )
+    if reason == "joint_fit_check_failed":
+        metric_names = {
+            "plane_rms_mm": "平面拟合RMS",
+            "radial_rms_mm": "圆半径拟合RMS",
+            "radius_mm": "拟合半径",
+            "image_radial_rms_px": "二维圆半径拟合RMS",
+            "image_radial_p95_px": "二维圆半径误差P95",
+            "image_radius_px": "二维拟合半径",
+            "arc_deg": "实测圆弧",
+            "monotonic_correction_deg": "最大单调修正",
+            "hysteresis_deg": "最大正反程差",
+            "cycle_travel_range_deg": "三轮行程差",
+            "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
+            "axis_plane_rms_mm": "三维圆轴向RMS",
+            "axis_radial_rms_mm": "三维圆半径RMS",
+            "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
+            "axis_cycle_difference_deg": "三轮转轴方向极差",
+            "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
+            "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
+            "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
+            "state_image_sync_p95_ms": "图像与电机状态同步误差P95",
+            "tag_valid_rate_percent": "所需Tag同时有效率",
+        }
+        metric_units = {
+            "plane_rms_mm": "mm",
+            "radial_rms_mm": "mm",
+            "radius_mm": "mm",
+            "image_radial_rms_px": "px",
+            "image_radial_p95_px": "px",
+            "image_radius_px": "px",
+            "arc_deg": "°",
+            "monotonic_correction_deg": "°",
+            "hysteresis_deg": "°",
+            "cycle_travel_range_deg": "°",
+            "rotation_orthogonal_rms_deg": "°",
+            "axis_plane_rms_mm": "mm",
+            "axis_radial_rms_mm": "mm",
+            "rotation_circle_axis_difference_deg": "°",
+            "axis_cycle_difference_deg": "°",
+            "third_cycle_axis_holdout_deg": "°",
+            "third_cycle_axis_line_rms_mm": "mm",
+            "third_cycle_trajectory_p95_deg": "°",
+            "state_image_sync_p95_ms": "ms",
+            "tag_valid_rate_percent": "%",
+        }
+        details: list[str] = []
+        for failure in active.get("failures", []):
+            joint = JOINT_NAMES_ZH.get(
+                str(failure.get("joint")), str(failure.get("joint"))
+            )
+            metric = str(failure.get("metric", ""))
+            if metric in metric_names:
+                comparison = str(failure.get("comparison", ""))
+                requirement = "不超过" if comparison == "maximum" else "至少"
+                unit = metric_units[metric]
+                detail = (
+                    f"{joint}的{metric_names[metric]}为"
+                    f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
+                    f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
+                )
+                cycle_travel = failure.get("cycle_travel_deg", [])
+                if cycle_travel:
+                    detail += "（三轮=" + "/".join(
+                        f"{float(value):.2f}°" for value in cycle_travel
+                    ) + "）"
+                details.append(detail)
+            else:
+                cycle = failure.get("cycle")
+                cycle_text = "" if cycle is None else f"第{cycle}轮"
+                details.append(
+                    f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
+                    f"{failure.get('reason', '未知原因')}"
+                )
+        detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
+        return (
+            detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
+            "修正Tag位置、遮挡或机械行程后调用"
+            "/g20_calibration/resume；程序只清除当前失败关节的数据"
+            f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
+        )
+    if reason == "zero_model_validation_failed":
+        reason_names = {
+            "zero_offset_reached_configured_bound": "零位解触及安全边界",
+            "zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
+            "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
+            "zero_offset_cycle_difference_too_large": "三轮零位离散过大",
+            "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
+            "zero_axis_cone_mismatch_too_large": (
+                "父子轴夹角与原始URDF不一致，零位旋转无法解释"
+            ),
+            "zero_phase_axis_line_residual_too_large": (
+                "整段SE(3)运动无法稳定确定平行轴线相位"
+            ),
+            "zero_offset_did_not_improve_with_95pct_confidence": (
+                "第三轮留出验证未以95%置信度改善"
+            ),
+        }
+        details: list[str] = []
+        for failure in active.get("failures", []):
+            joint = JOINT_NAMES_ZH.get(
+                str(failure.get("joint")), str(failure.get("joint"))
+            )
+            if failure.get("metric") == "zero_guard":
+                reason_text = reason_names.get(
+                    str(failure.get("reason")), str(failure.get("reason"))
+                )
+                if "actual_deg" in failure and "limit_deg" in failure:
+                    reason_text += (
+                        f"（估计{float(failure['actual_deg']):+.2f}°，"
+                        f"允许±{float(failure['limit_deg']):.2f}°）"
+                    )
+                details.append(f"{joint}：{reason_text}")
+        return (
+            "轨迹采集已完成，但零位/URDF几何验证失败"
+            + ("（" + "；".join(details) + "）" if details else "")
+            + "。程序没有生成正式JSON或修正URDF。",
+            "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
+            "请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
+        )
+    if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
+        return (
+            "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
+            "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
+        )
+    if reason == "call_start":
+        return (
+            "三机位预检已经通过，等待操作员确认开始。",
+            "清空机械手运动范围后调用/g20_calibration/start。",
+        )
+    if reason == "operator_pause":
+        return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
+    if reason == "operator_abort":
+        return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
+    if reason == "collecting_timestamp_synchronised_tag_centres":
+        return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
+    if reason == "capturing_random_validation_pose":
+        return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
+    if reason in {"calibration_passed", "calibration_complete"}:
+        return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
+    if reason == "quality_failed":
+        return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
+    if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
+        return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
+    if state == "RETURN_BASELINE":
+        return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
+    if state == "FITTING":
+        return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
+    return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
+
+
+def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
+    """Render the complete operator status; the JSON topic remains unchanged."""
+    state = str(payload.get("state", ""))
+    active = payload.get("active", {})
+    reason_zh, action_zh = three_camera_reason_zh(
+        state, str(payload.get("reason", "")), active
+    )
+    service_prefix = str(payload.get("service_prefix", "/g20_calibration"))
+    if service_prefix != "/g20_calibration":
+        reason_zh = reason_zh.replace("/g20_calibration", service_prefix)
+        action_zh = action_zh.replace("/g20_calibration", service_prefix)
+    progress = float(payload.get("progress", 0.0))
+    completed = payload.get("completed_sweeps", 0)
+    total = payload.get("total_sweeps", 0)
+    executed = int(payload.get("executed_sweep_directions", completed))
+    scan_progress = float(
+        payload.get(
+            "scan_progress",
+            0.0 if not total else float(completed) / float(total),
+        )
+    )
+    lines = [
+        f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
+        f"原因：{reason_zh}",
+        f"建议：{action_zh}",
+        f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
+        f"扫描进度{scan_progress:.1%}）",
+        f"当前任务：{_task_text(active)}",
+    ]
+    if executed > int(completed):
+        lines.append(
+            f"实际采集：已启动{executed}个方向（含自动重扫）；"
+            f"计划进度只统计{total}个唯一方向，重扫不会重复增加计划进度"
+        )
+    attempt = int(active.get("attempt", active.get("fit_attempt", 1)))
+    if attempt > 1:
+        lines.append(
+            f"整关节重采：当前为第{attempt}/"
+            f"{active.get('fit_attempt_limit', '?')}次采集结果"
+        )
+    if state == "RETURN_BASELINE":
+        baseline_command = payload.get("baseline_command_u8", [])
+        return_command = payload.get("return_command_u8", baseline_command)
+        label = "恢复姿态" if return_command != baseline_command else "基准姿态"
+        lines.append(f"正在确认{label}：{return_command}")
+    if active and active.get("kind") not in {
+        "fit_failure",
+        "zero_model_failure",
+        "motion_stall",
+    }:
+        retry_count = int(active.get("automatic_retry_count", 0))
+        if retry_count:
+            lines.append(
+                "自动重试：当前方向已自动重扫"
+                f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
+                f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
+                f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
+            )
+        sample = active.get("sample", {})
+        motion_progress = active.get("motion_progress")
+        motion_text = (
+            "未知" if motion_progress is None else f"{float(motion_progress):.1%}"
+        )
+        lines.append(
+            "运动采样："
+            f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
+            f"实际{_format_u8(active.get('actual_u8'))}，"
+            f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
+            f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
+            f"{_format_u8(sample.get('maximum_u8'))}"
+        )
+        auxiliary = active.get("auxiliary_motors", [])
+        if auxiliary:
+            lines.append(
+                "避挡姿态："
+                + "，".join(
+                    f"电机{item.get('motor_index')}目标"
+                    f"{item.get('command_u8')}、实际"
+                    f"{_format_u8(item.get('actual_u8'))}"
+                    for item in auxiliary
+                )
+            )
+        speed = active.get("speed", {})
+        if speed:
+            lines.append(
+                "阶段速度：五指目标"
+                f"{speed.get('commanded_finger_speed')}，SDK报告"
+                f"{speed.get('reported_finger_speed')}"
+            )
+        if active.get("sweep_timeout_seconds") is not None:
+            lines.append(
+                "运动保护：扫描超时"
+                f"{float(active['sweep_timeout_seconds']):.1f}s，"
+                "连续"
+                f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
+                "进展不足"
+                f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
+                "则立即暂停"
+            )
+    lines.append("机位：")
+    for name, view in payload.get("views", {}).items():
+        missing = view.get("missing_tag_ids", [])
+        missing_text = "无" if not missing else ",".join(map(str, missing))
+        lines.append(
+            f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
+            f"{'就绪' if view.get('ready') else '等待'}，"
+            f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，"
+            f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
+            f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
+            f"当前缺失Tag={missing_text}"
+        )
+    extrinsics_error = payload.get("camera_extrinsics_error")
+    if extrinsics_error:
+        lines.append(f"外参文件：{extrinsics_error}")
+    lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
+    lines.append(
+        f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
+    )
+    return "\n".join(lines)

src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py

@@ -0,0 +1,36 @@
+"""Publish calibrated URDF joint angles from raw u8 commands."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("hand_model", default_value="G20"),
+            DeclareLaunchArgument("hand_type", default_value="right"),
+            DeclareLaunchArgument("calibration_file"),
+            DeclareLaunchArgument("input_topic", default_value=""),
+            DeclareLaunchArgument("output_topic", default_value=""),
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="calibrated_joint_state_bridge",
+                name="calibrated_joint_state_bridge",
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "hand_model": LaunchConfiguration("hand_model"),
+                        "hand_type": LaunchConfiguration("hand_type"),
+                        "calibration_file": LaunchConfiguration(
+                            "calibration_file"
+                        ),
+                        "input_topic": LaunchConfiguration("input_topic"),
+                        "output_topic": LaunchConfiguration("output_topic"),
+                    }
+                ],
+            ),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py

@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -48,29 +47,41 @@ def _launch_stack(context):
 
     tag_config = LaunchConfiguration("tag_config").perform(context)
     zero_config = LaunchConfiguration("zero_config").perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
-    realsense_launch = (
-        Path(get_package_share_directory("realsense2_camera"))
-        / "launch"
-        / "rs_launch.py"
-    )
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": "false",
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     raw_topic = "/camera/camera/color/image_raw"
@@ -196,10 +207,39 @@ def generate_launch_description() -> LaunchDescription:
     )
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
             DeclareLaunchArgument("can_interface", default_value="can0"),
             DeclareLaunchArgument("calibration_speed", default_value="15"),
@@ -221,12 +261,6 @@ def generate_launch_description() -> LaunchDescription:
                     package_share / "config" / "cmc_pitch_zero.yaml"
                 ),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(

src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py

@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -50,29 +49,41 @@ def _launch_stack(context):
     calibration_config = LaunchConfiguration(
         "calibration_config"
     ).perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
-    realsense_launch = (
-        Path(get_package_share_directory("realsense2_camera"))
-        / "launch"
-        / "rs_launch.py"
-    )
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": "false",
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     raw_topic = "/camera/camera/color/image_raw"
@@ -198,10 +209,39 @@ def generate_launch_description() -> LaunchDescription:
     )
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
             DeclareLaunchArgument("can_interface", default_value="can0"),
             DeclareLaunchArgument("calibration_speed", default_value="15"),
@@ -225,12 +265,6 @@ def generate_launch_description() -> LaunchDescription:
                     / "cmc_roll_zero_travel.yaml"
                 ),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(

src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py

@@ -11,12 +11,11 @@ from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     ExecuteProcess,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -48,7 +47,6 @@ def _launch_stack(context):
 
     calibration_config = LaunchConfiguration("calibration_config").perform(context)
     tag_config = LaunchConfiguration("tag_config").perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
     use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
     if use_roi_text not in {"true", "false"}:
         raise RuntimeError("use_roi must be true or false")
@@ -66,47 +64,58 @@ def _launch_stack(context):
     if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
         raise RuntimeError("roi_width and roi_height must be positive")
 
-    color_profile = LaunchConfiguration("color_profile").perform(context)
-    profile_match = re.fullmatch(r"(\d+)x(\d+)x(\d+)", color_profile)
-    if use_roi and profile_match is not None:
-        image_width = int(profile_match.group(1))
-        image_height = int(profile_match.group(2))
+    try:
+        image_width = int(LaunchConfiguration("image_width").perform(context))
+        image_height = int(LaunchConfiguration("image_height").perform(context))
+    except ValueError as error:
+        raise RuntimeError("image_width and image_height must be integers") from error
+    if image_width <= 0 or image_height <= 0:
+        raise RuntimeError("image_width and image_height must be positive")
+    if use_roi:
         if (
             roi_values["roi_x"] + roi_values["roi_width"] > image_width
             or roi_values["roi_y"] + roi_values["roi_height"] > image_height
         ):
             raise RuntimeError(
-                "ROI lies outside color_profile "
+                "ROI lies outside camera image "
                 f"{image_width}x{image_height}"
             )
 
-    realsense_launch = Path(
-        get_package_share_directory("realsense2_camera")
-    ) / "launch" / "rs_launch.py"
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            # D405 exposes color from the stereo/depth module. Keep the RGB
-            # camera argument as well so the same launch file also works with
-            # D435/D455-class devices.
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": LaunchConfiguration("enable_depth"),
-            "depth_module.depth_profile": LaunchConfiguration("depth_profile"),
-            # Depth is archival/diagnostic only. Synchronising and aligning it
-            # adds significant D405 processing latency without affecting the
-            # relative AprilTag rotation calculation.
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     vision_components = []
@@ -275,7 +284,6 @@ def _launch_stack(context):
             str(bag_path),
             processed_image_raw_topic,
             processed_camera_info_topic,
-            "/camera/camera/depth/image_rect_raw",
             "/apriltag/detections",
             "/tf",
             "/g20/cb_left_hand_control_cmd",
@@ -312,15 +320,40 @@ def generate_launch_description() -> LaunchDescription:
     default_output = str(Path.cwd() / "calibration_output")
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
             ),
             DeclareLaunchArgument(
-                "depth_profile", default_value="640x480x30"
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
-            DeclareLaunchArgument("enable_depth", default_value="false"),
             DeclareLaunchArgument(
                 "publish_debug_image", default_value="false"
             ),
@@ -349,12 +382,6 @@ def generate_launch_description() -> LaunchDescription:
                 "calibration_config",
                 default_value=str(package_share / "config" / "calibration.yaml"),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(package_share / "config" / "front_tags.yaml"),

src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py

@@ -0,0 +1,519 @@
+"""Launch three Hikrobot views and one supported-hand calibration owner."""
+
+from __future__ import annotations
+
+from datetime import datetime
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    get_hand_calibration_profile,
+)
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _default_source_urdf(hand_model: str, hand_type: str) -> Path:
+    if hand_model == "O30":
+        relative = (
+            Path("linkerhand-urdf/O30/urdf_0803-right/src")
+            / "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
+        )
+        candidates = (
+            Path.cwd().parent / relative,
+            Path.home() / "projects" / relative,
+        )
+        return next(
+            (candidate for candidate in candidates if candidate.is_file()),
+            Path.home()
+            / "projects"
+            / "linkerhand-urdf/O30/urdf_0803-right/src"
+            / "linkerhand_O30i_right.urdf"
+            / "linkerhand_O30i_right-0803.urdf",
+        )
+    relative = Path(
+        "assets/robots/hands/linker_hand"
+    ) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
+    workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
+    try:
+        installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
+    except Exception:
+        installed = workspace
+    return workspace if workspace.is_file() else installed
+
+
+def _launch_stack(context):
+    hand_model = LaunchConfiguration("hand_model").perform(context).upper()
+    hand_type = LaunchConfiguration("hand_type").perform(context).lower()
+    try:
+        profile = get_hand_calibration_profile(hand_type, hand_model)
+    except ValueError as error:
+        raise RuntimeError(str(error)) from error
+    model_key = hand_model.lower()
+    calibration_namespace = f"/{model_key}_calibration"
+    if hand_model == "O30":
+        command_topic = f"/cb_{hand_type}_hand_control_cmd"
+        state_topic = f"/cb_{hand_type}_hand_state"
+        hand_info_topic = f"/cb_{hand_type}_hand_info"
+        setting_topic = "/cb_hand_setting_cmd"
+        tag_config = LaunchConfiguration("o30_tag_config")
+    else:
+        command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
+        state_topic = f"/g20/cb_{hand_type}_hand_state"
+        hand_info_topic = f"/g20/cb_{hand_type}_hand_info"
+        setting_topic = "/g20/cb_hand_setting_cmd"
+        tag_config = LaunchConfiguration("tag_config")
+    requested_source = LaunchConfiguration("source_urdf_path").perform(context)
+    source_urdf = (
+        Path(requested_source).expanduser().resolve()
+        if requested_source
+        else _default_source_urdf(hand_model, hand_type).resolve()
+    )
+    if not source_urdf.is_file():
+        raise RuntimeError(f"source URDF does not exist: {source_urdf}")
+
+    hand_serial = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not hand_serial
+        or hand_serial == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None
+        or hand_serial in {".", ".."}
+    ):
+        raise RuntimeError("serial_number must be a safe non-empty hand serial")
+
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(
+        LaunchConfiguration("output_root").perform(context)
+    ).expanduser().resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        session_dir = (
+            output_root
+            / hand_serial
+            / datetime.now().strftime("%Y%m%d_%H%M%S")
+        )
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    camera_serials = {
+        view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        for view in VIEWS
+    }
+    if any(not serial for serial in camera_serials.values()):
+        raise RuntimeError("all three camera serial numbers are required")
+    if len(set(camera_serials.values())) != 3:
+        raise RuntimeError("front/side/top camera serial numbers must be unique")
+
+    cameras = []
+    components = []
+    raw_topics = []
+    info_topics = []
+    detection_topics = []
+    for view in VIEWS:
+        namespace = f"{calibration_namespace}/{view}/camera"
+        raw_topic = f"{namespace}/image_raw"
+        camera_info_topic = f"{namespace}/camera_info"
+        rect_topic = f"{namespace}/image_rect"
+        detector_namespace = f"{calibration_namespace}/{view}/apriltag"
+        detection_topic = f"{detector_namespace}/detections"
+        raw_topics.append(raw_topic)
+        info_topics.append(camera_info_topic)
+        detection_topics.append(detection_topic)
+        cameras.append(
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                condition=IfCondition(LaunchConfiguration("start_cameras")),
+                parameters=[
+                    {
+                        "serial_number": LaunchConfiguration(
+                            f"{view}_camera_serial"
+                        ),
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": (
+                            f"{model_key}_calibration_{view}_optical_frame"
+                        ),
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": ParameterValue(
+                            LaunchConfiguration("auto_exposure"), value_type=bool
+                        ),
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        components.extend(
+            [
+                ComposableNode(
+                    package="image_proc",
+                    plugin="image_proc::RectifyNode",
+                    name=f"rectify_{view}",
+                    namespace=namespace,
+                    remappings=[
+                        ("image", raw_topic),
+                        ("camera_info", camera_info_topic),
+                        ("image_rect", rect_topic),
+                    ],
+                    parameters=[{"queue_size": 1}],
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                ),
+                ComposableNode(
+                    package="apriltag_ros",
+                    plugin="AprilTagNode",
+                    name="apriltag",
+                    namespace=detector_namespace,
+                    parameters=[
+                        tag_config,
+                        {
+                            "detector.decimate": ParameterValue(
+                                LaunchConfiguration("apriltag_decimate"),
+                                value_type=float,
+                            )
+                        },
+                    ],
+                    remappings=[
+                        ("image_rect", rect_topic),
+                        ("camera_info", camera_info_topic),
+                    ],
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                ),
+            ]
+        )
+
+    vision = ComposableNodeContainer(
+        name=f"{model_key}_three_camera_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=components,
+        output="screen",
+        emulate_tty=True,
+    )
+    if hand_model == "O30":
+        sdk = Node(
+            package="linker_hand_o30_ros2_sdk",
+            executable="linker_hand_o30_ros2_sdk",
+            name="linker_hand_o30_ros2_sdk",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[
+                {
+                    "hand_type": hand_type,
+                    "hand_joint": "O30",
+                    "is_touch": False,
+                    "canfd_device": ParameterValue(
+                        LaunchConfiguration("canfd_device"), value_type=int
+                    ),
+                    "comm_type": LaunchConfiguration("o30_comm_type"),
+                    "channel": LaunchConfiguration("can_interface"),
+                    "bitrate": ParameterValue(
+                        LaunchConfiguration("o30_bitrate"), value_type=int
+                    ),
+                    "dbitrate": ParameterValue(
+                        LaunchConfiguration("o30_dbitrate"), value_type=int
+                    ),
+                    "auto_setup": ParameterValue(
+                        LaunchConfiguration("o30_auto_setup"), value_type=bool
+                    ),
+                }
+            ],
+        )
+    else:
+        sdk = Node(
+            package="linker_hand_ros2_sdk",
+            executable="linker_hand_sdk",
+            name="linker_hand_sdk",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[
+                {
+                    "hand_type": hand_type,
+                    "hand_joint": "G20",
+                    "can": LaunchConfiguration("can_interface"),
+                    "modbus": "None",
+                    "topic_prefix": "/g20",
+                    "move_on_startup": False,
+                    "startup_speed": ParameterValue(
+                        LaunchConfiguration("calibration_speed"), value_type=int
+                    ),
+                    "startup_torque": 80,
+                    "state_poll_rate": 30.0,
+                    "velocity_poll_rate": 1.0,
+                    "defer_state_reads_while_commanding": False,
+                    "repeat_position_commands": False,
+                    "is_touch": False,
+                }
+            ],
+        )
+    calibration = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="three_camera_calibration_node",
+        name=f"{model_key}_calibration",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            LaunchConfiguration("calibration_config"),
+            {
+                "serial_number": hand_serial,
+                "hand_model": hand_model,
+                "hand_type": hand_type,
+                "session_dir": str(session_dir),
+                # The SDK performs roughly 25 synchronous CAN queries whenever
+                # cb_<side>_hand_info has a subscriber. Calibration only used
+                # that topic to display a speed diagnostic, while those reads
+                # created 17-33 command-unit holes in position trajectories.
+                "info_topic": f"{calibration_namespace}/disabled_hand_info",
+                "command_topic": command_topic,
+                "state_topic": state_topic,
+                "setting_topic": setting_topic,
+                "baseline_command_u8": list(profile.baseline_command),
+                "camera_extrinsics_file": LaunchConfiguration(
+                    "camera_extrinsics_file"
+                ),
+                "source_urdf_path": str(source_urdf),
+                "corrected_urdf_output_dir": LaunchConfiguration(
+                    "corrected_urdf_output_dir"
+                ),
+                **{
+                    f"{view}_camera_serial": camera_serials[view]
+                    for view in VIEWS
+                },
+                **{
+                    f"{view}_camera_info_topic": info_topics[index]
+                    for index, view in enumerate(VIEWS)
+                },
+                **{
+                    f"{view}_detections_topic": detection_topics[index]
+                    for index, view in enumerate(VIEWS)
+                },
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"), value_type=bool
+                ),
+                "normal_calibration_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"), value_type=int
+                ),
+                "index_roll_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_roll_calibration_speed"),
+                    value_type=int,
+                ),
+                "index_flex_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_flex_calibration_speed"),
+                    value_type=int,
+                ),
+                "o30_internal_speed_u8": ParameterValue(
+                    LaunchConfiguration("o30_internal_speed_u8"), value_type=int
+                ),
+                "o30_command_full_range_seconds": ParameterValue(
+                    LaunchConfiguration("o30_command_full_range_seconds"),
+                    value_type=float,
+                ),
+                "validation_enabled": ParameterValue(
+                    LaunchConfiguration("validation_enabled"), value_type=bool
+                ),
+            },
+        ],
+    )
+    bag = ExecuteProcess(
+        condition=IfCondition(LaunchConfiguration("record_bag")),
+        cmd=[
+            "ros2",
+            "bag",
+            "record",
+            "--storage",
+            "mcap",
+            "--storage-preset-profile",
+            "zstd_fast",
+            "--max-bag-size",
+            "10737418240",
+            "--output",
+            str(session_dir / "rosbag"),
+            *raw_topics,
+            *info_topics,
+            *detection_topics,
+            command_topic,
+            state_topic,
+            hand_info_topic,
+            f"{calibration_namespace}/status",
+        ],
+        output="screen",
+    )
+    return [
+        LogInfo(
+            msg=(
+                f"{hand_model} {hand_type} three-camera session: {session_dir}; "
+                f"source_urdf={source_urdf}"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Camera mapping: front="
+                f"{camera_serials['front']} side={camera_serials['side']} "
+                f"top={camera_serials['top']}"
+            )
+        ),
+        *cameras,
+        vision,
+        sdk,
+        calibration,
+        bag,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    info_root = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Camera processes publish ~2 MB frames across DDS.  Force the
+            # matching RMW and provide both current and legacy profile names
+            # so the configured 64 MB shared-memory segment is actually used.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            DeclareLaunchArgument("hand_model", default_value="G20"),
+            DeclareLaunchArgument("hand_type", default_value="left"),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "front_camera_serial", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_serial", default_value="DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_serial", default_value="DB2163739"
+            ),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument(
+                "front_camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_name", default_value="hikrobot_side_DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_name", default_value="hikrobot_top_DB2163739"
+            ),
+            DeclareLaunchArgument(
+                "front_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163742.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "side_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163749.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "top_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163739.yaml"),
+            ),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("canfd_device", default_value="0"),
+            DeclareLaunchArgument("o30_comm_type", default_value="socketcan"),
+            DeclareLaunchArgument("o30_bitrate", default_value="1000000"),
+            DeclareLaunchArgument("o30_dbitrate", default_value="5000000"),
+            DeclareLaunchArgument("o30_auto_setup", default_value="true"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument(
+                "index_roll_calibration_speed", default_value="5"
+            ),
+            DeclareLaunchArgument(
+                "index_flex_calibration_speed", default_value="10"
+            ),
+            DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
+            DeclareLaunchArgument(
+                "o30_command_full_range_seconds", default_value="6.0"
+            ),
+            DeclareLaunchArgument("validation_enabled", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_extrinsics_file",
+                default_value=str(
+                    Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "source_urdf_path", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "corrected_urdf_output_dir", default_value=""
+            ),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument("start_cameras", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument("record_bag", default_value="false"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_calibration.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_tags.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "o30_tag_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_tags_o30.yaml"
+                ),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py

@@ -0,0 +1,199 @@
+"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _launch(context):
+    cameras = []
+    rectifiers = []
+    serials = {}
+    for view in VIEWS:
+        serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        if not serial:
+            raise RuntimeError(f"{view}_camera_serial is required")
+        serials[view] = serial
+        namespace = f"/g20_extrinsics/{view}/camera"
+        cameras.append(
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "serial_number": serial,
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": f"g20_extrinsics_{view}_optical_frame",
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": False,
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        rectifiers.append(
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name=f"rectify_{view}",
+                namespace=namespace,
+                remappings=[
+                    ("image", f"{namespace}/image_raw"),
+                    ("camera_info", f"{namespace}/camera_info"),
+                    ("image_rect", f"{namespace}/image_rect"),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            )
+        )
+    container = ComposableNodeContainer(
+        name="g20_extrinsics_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=rectifiers,
+        output="screen",
+    )
+    solver = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="three_camera_extrinsics_node",
+        name="g20_camera_extrinsics",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {
+                "output_file": LaunchConfiguration("output_file"),
+                "checkerboard_columns": ParameterValue(
+                    LaunchConfiguration("checkerboard_columns"), value_type=int
+                ),
+                "checkerboard_rows": ParameterValue(
+                    LaunchConfiguration("checkerboard_rows"), value_type=int
+                ),
+                "square_size_m": ParameterValue(
+                    LaunchConfiguration("square_size_m"), value_type=float
+                ),
+                "enable_gui": ParameterValue(
+                    LaunchConfiguration("enable_gui"), value_type=bool
+                ),
+                "gui_refresh_hz": ParameterValue(
+                    LaunchConfiguration("gui_refresh_hz"), value_type=float
+                ),
+                "maximum_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration("maximum_reprojection_rms_px"),
+                    value_type=float,
+                ),
+                "maximum_single_camera_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration(
+                        "maximum_single_camera_reprojection_rms_px"
+                    ),
+                    value_type=float,
+                ),
+                "auto_capture_default": ParameterValue(
+                    LaunchConfiguration("auto_capture_default"),
+                    value_type=bool,
+                ),
+                "auto_capture_stable_seconds": ParameterValue(
+                    LaunchConfiguration("auto_capture_stable_seconds"),
+                    value_type=float,
+                ),
+                **{
+                    f"{view}_camera_serial": serials[view]
+                    for view in VIEWS
+                },
+            }
+        ],
+    )
+    return [*cameras, container, solver]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    camera_info = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Keep the large-image transport deterministic even when the
+            # calling shell selected another ROS 2 RMW implementation.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("front_camera_serial", default_value="DB2163742"),
+            DeclareLaunchArgument("side_camera_serial", default_value="DB2163749"),
+            DeclareLaunchArgument("top_camera_serial", default_value="DB2163739"),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument("front_camera_name", default_value="hikrobot_front_DB2163742"),
+            DeclareLaunchArgument("side_camera_name", default_value="hikrobot_side_DB2163749"),
+            DeclareLaunchArgument("top_camera_name", default_value="hikrobot_top_DB2163739"),
+            DeclareLaunchArgument("front_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163742.yaml")),
+            DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
+            DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
+            DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("checkerboard_columns", default_value="8"),
+            DeclareLaunchArgument("checkerboard_rows", default_value="5"),
+            DeclareLaunchArgument("square_size_m", default_value="0.027"),
+            DeclareLaunchArgument("enable_gui", default_value="true"),
+            DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
+            DeclareLaunchArgument(
+                "maximum_reprojection_rms_px", default_value="1.2"
+            ),
+            DeclareLaunchArgument(
+                "maximum_single_camera_reprojection_rms_px",
+                default_value="1.5",
+            ),
+            DeclareLaunchArgument("auto_capture_default", default_value="false"),
+            DeclareLaunchArgument(
+                "auto_capture_stable_seconds", default_value="1.0"
+            ),
+            DeclareLaunchArgument(
+                "output_file",
+                default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
+            ),
+            OpaqueFunction(function=_launch),
+        ]
+    )

src/g20_thumb_apriltag_calibration/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>g20_thumb_apriltag_calibration</name>
   <version>0.1.0</version>
-  <description>Front RealSense AprilTag calibration for the left G20 thumb.</description>
+  <description>Three-view Hikrobot AprilTag calibration for G20 hands and the O30 right hand.</description>
   <maintainer email="support@linker-robotics.com">lxp</maintainer>
   <license>MIT</license>
 
@@ -15,9 +15,9 @@
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>linker_hand_ros2_sdk</exec_depend>
+  <exec_depend>linker_hand_o30_ros2_sdk</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>rclpy</exec_depend>
-  <exec_depend>realsense2_camera</exec_depend>
   <exec_depend>rosbag2</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
@@ -26,6 +26,7 @@
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-opencv</exec_depend>
   <exec_depend>python3-scipy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>python3-pytest</test_depend>
 

src/g20_thumb_apriltag_calibration/setup.py

@@ -15,18 +15,25 @@ setup(
             ["resource/" + package_name],
         ),
         ("share/" + package_name, ["package.xml", "README.md"]),
-        ("share/" + package_name + "/config", glob("config/*.yaml")),
+        (
+            "share/" + package_name + "/config",
+            glob("config/*.yaml") + glob("config/*.xml"),
+        ),
         ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
     ],
-    install_requires=["setuptools", "numpy", "scipy"],
+    install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
     tests_require=["pytest"],
     zip_safe=True,
     maintainer="lxp",
     maintainer_email="support@linker-robotics.com",
-    description="Front RealSense AprilTag calibration for the left G20 thumb",
+    description="Three-view AprilTag calibration for G20 and O30 hands",
     license="MIT",
     entry_points={
         "console_scripts": [
+            (
+                "hikrobot_camera_node = "
+                "g20_thumb_apriltag_calibration.hikrobot_camera:main"
+            ),
             "calibration_node = g20_thumb_apriltag_calibration.node:main",
             (
                 "cmc_pitch_zero_node = "
@@ -36,6 +43,27 @@ setup(
                 "cmc_roll_calibration_node = "
                 "g20_thumb_apriltag_calibration.zero_node:main"
             ),
+            (
+                "three_camera_calibration_node = "
+                "g20_thumb_apriltag_calibration.three_camera_node:main"
+            ),
+            (
+                "three_camera_extrinsics_node = "
+                "g20_thumb_apriltag_calibration.extrinsics_node:main"
+            ),
+            (
+                "offline_replay = "
+                "g20_thumb_apriltag_calibration.offline_replay:main"
+            ),
+            (
+                "camera_alignment_view = "
+                "g20_thumb_apriltag_calibration.alignment_view:main"
+            ),
+            (
+                "calibrated_joint_state_bridge = "
+                "g20_thumb_apriltag_calibration."
+                "calibrated_joint_state_bridge:main"
+            ),
         ],
     },
 )

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

@@ -15,11 +15,60 @@ from g20_thumb_apriltag_calibration.acquisition import (
     aggregate_observations,
     aggregate_sweep_observations,
     interpolate_state_u8,
+    required_resume_views,
     tag_quality_is_valid,
+    update_pnp_reset_watchdog,
 )
 from g20_thumb_apriltag_calibration.core import PAIR_NAMES
 
 
+def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
+    since, reset = update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=False,
+        now=10.0,
+        invalid_since=None,
+        reset_after_seconds=1.0,
+    )
+    assert since == 10.0
+    assert reset is False
+
+    since, reset = update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=False,
+        now=11.01,
+        invalid_since=since,
+        reset_after_seconds=1.0,
+    )
+    assert since == 11.01
+    assert reset is True
+
+
+def test_pnp_watchdog_clears_on_valid_pose_or_bad_detection() -> None:
+    assert update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=True,
+        now=11.0,
+        invalid_since=10.0,
+        reset_after_seconds=1.0,
+    ) == (None, False)
+    assert update_pnp_reset_watchdog(
+        detection_good=False,
+        pnp_valid=False,
+        now=11.0,
+        invalid_since=10.0,
+        reset_after_seconds=1.0,
+    ) == (None, False)
+
+
+def test_resume_requires_only_the_active_view() -> None:
+    assert required_resume_views("top") == ("top",)
+    assert required_resume_views("front") == ("front",)
+    assert required_resume_views(None) == ("front", "side", "top")
+    with pytest.raises(ValueError, match="unknown"):
+        required_resume_views("rear")
+
+
 def _observation(index: int, angle_rad: float = 0.0) -> Observation:
     quaternion = tuple(
         float(value)

src/g20_thumb_apriltag_calibration/test/test_alignment_view.py

@@ -0,0 +1,39 @@
+"""Tests for the independent physical-line alignment overlay."""
+
+import pytest
+
+from g20_thumb_apriltag_calibration.alignment_view import (
+    summarize_alignment_measurements,
+)
+
+
+def _measurement(angle: float, offset: float, y: float) -> dict:
+    return {
+        "line_xyxy_px": [0.0, y, 100.0, y - angle * 100.0],
+        "angle_rad": angle,
+        "vertical_offset_px": offset,
+    }
+
+
+def test_line_summary_smooths_only_physical_line_measurements() -> None:
+    """The overlay smooths scene lines without any Tag orientation input."""
+    result = summarize_alignment_measurements(
+        [
+            _measurement(-0.02, -4.0, 80.0),
+            None,
+            _measurement(0.00, 0.0, 82.0),
+            _measurement(0.02, 4.0, 84.0),
+        ]
+    )
+
+    assert result is not None
+    assert result["angle_rad"] == pytest.approx(0.0)
+    assert result["vertical_offset_px"] == pytest.approx(0.0)
+    assert result["line_xyxy_px"] == pytest.approx([0.0, 82.0, 100.0, 82.0])
+    assert result["detected_frames"] == 3
+    assert result["window_frames"] == 4
+
+
+def test_line_summary_returns_none_without_scene_line() -> None:
+    """No blue line is fabricated when the scene has no valid long edge."""
+    assert summarize_alignment_measurements([None, None]) is None

src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py

@@ -0,0 +1,131 @@
+import copy
+
+import pytest
+
+from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
+    G20_COMMAND_NAMES,
+    G20_URDF_JOINT_NAMES,
+    O30_COMMAND_NAMES,
+    O30_URDF_JOINT_NAMES,
+    CalibratedCommandMapper,
+)
+from g20_thumb_apriltag_calibration.full_hand import (
+    JointCurveFit,
+    build_compact_payload,
+    get_hand_calibration_profile,
+)
+
+
+def _payload(side: str = "right") -> dict:
+    profile = get_hand_calibration_profile(side)
+    curve = [round((255 - command) * 0.001, 8) for command in range(256)]
+    joints = {}
+    for name, spec in profile.joint_specs.items():
+        joint = {
+            "motor_index": spec.motor_index,
+            "angle_rad": list(curve),
+        }
+        if spec.active:
+            joint["zero_command_u8"] = 255
+            joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
+        else:
+            joint["passive"] = True
+        if spec.source_joint is not None:
+            joint["source_joint"] = spec.source_joint
+        joints[name] = joint
+    return {
+        "schema_version": 4,
+        "model": "G20",
+        "side": side,
+        "serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
+        "angle_unit": "rad",
+        "command_range": [0, 255],
+        "baseline_command_u8": [255] * 20,
+        "joints": joints,
+        "quality": {
+            "passed": True,
+            "validation_mae_rad": 0.01,
+            "validation_p95_rad": 0.02,
+        },
+    }
+
+
+def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    command = list(range(20))
+    result = dict(
+        zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
+    )
+
+    assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
+    assert result["thumb_cmc_roll"] == pytest.approx(0.250)
+    assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
+    assert result["thumb_mcp"] == pytest.approx(0.240)
+    assert result["thumb_ip"] == pytest.approx(0.240)
+    assert result["pinky_pip"] == pytest.approx(0.236)
+    assert result["pinky_dip"] == pytest.approx(0.236)
+
+
+def test_mapper_uses_names_instead_of_message_order() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    command = list(range(20))
+    names = list(reversed(G20_COMMAND_NAMES))
+    positions = list(reversed(command))
+
+    assert mapper.map_positions(positions, names) == mapper.map_positions(command)
+
+
+def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
+    with pytest.raises(ValueError, match="does not match"):
+        CalibratedCommandMapper(_payload("left"), expected_side="right")
+
+    payload = copy.deepcopy(_payload())
+    payload["quality"]["passed"] = False
+    with pytest.raises(ValueError, match="quality.passed"):
+        CalibratedCommandMapper(payload, expected_side="right")
+
+
+def test_mapper_rejects_incomplete_named_command() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    with pytest.raises(ValueError, match="missing named channels"):
+        mapper.map_positions([255.0], ["thumb_cmc_pitch"])
+
+
+def test_o30_mapper_uses_sdk_names_and_twenty_active_urdf_joints() -> None:
+    profile = get_hand_calibration_profile("right", "O30")
+
+    def fit_for(name: str) -> JointCurveFit:
+        zero = profile.baseline_command[profile.joint_specs[name].motor_index]
+        curve = tuple((command - zero) * 0.001 for command in range(256))
+        return JointCurveFit(
+            angle_rad=curve,
+            decreasing_rad=curve,
+            increasing_rad=curve,
+            circle={},
+            maximum_monotonic_correction_rad=0.0,
+            maximum_hysteresis_rad=0.0,
+            quality={},
+        )
+
+    payload = build_compact_payload(
+        serial_number="O30_RIGHT_TEST",
+        measured_fits={name: fit_for(name) for name in profile.measured_joints},
+        urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
+        validation_errors_rad=[0.0],
+        passed=True,
+        side="right",
+        model="O30",
+    )
+    mapper = CalibratedCommandMapper(payload, expected_side="right")
+
+    assert mapper.model == "O30"
+    assert mapper.command_names == O30_COMMAND_NAMES
+    assert mapper.urdf_joint_names == O30_URDF_JOINT_NAMES
+    baseline = profile.baseline_command
+    assert mapper.map_positions(baseline, O30_COMMAND_NAMES) == pytest.approx(
+        [0.0] * 20
+    )
+    moved = list(baseline)
+    moved[19] = 100
+    result = dict(zip(O30_URDF_JOINT_NAMES, mapper.map_positions(moved)))
+    assert result["pinky_dip"] == pytest.approx(0.1)

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -1,4 +1,5 @@
 from pathlib import Path
+from xml.etree import ElementTree
 
 import yaml
 
@@ -6,6 +7,36 @@ import yaml
 PACKAGE_ROOT = Path(__file__).resolve().parents[1]
 
 
+def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
+    root = ElementTree.parse(
+        PACKAGE_ROOT / "config" / "fastdds_large_images.xml"
+    ).getroot()
+    namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"}
+    profiles = root.find("dds:profiles", namespace)
+    assert profiles is not None
+    segment = profiles.find(
+        ".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size",
+        namespace,
+    )
+    assert segment is not None
+    assert int(segment.text) >= 64 * 1024 * 1024
+    participant = profiles.find("dds:participant", namespace)
+    assert participant is not None
+    assert participant.attrib["is_default_profile"] == "true"
+
+
+def test_three_camera_launches_force_fastdds_large_image_transport() -> None:
+    for launch_name in (
+        "three_camera_calibration.launch.py",
+        "three_camera_extrinsics.launch.py",
+    ):
+        launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text()
+        assert 'name="RMW_IMPLEMENTATION"' in launch_text
+        assert 'value="rmw_fastrtps_cpp"' in launch_text
+        assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text
+        assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text
+
+
 def test_front_tag_parameters_match_namespaced_detector() -> None:
     config = yaml.safe_load(
         (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
@@ -24,6 +55,141 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
     assert detector["detector"]["debug"] is False
 
 
+def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
+    expected = {
+        "front": [0, 1, 2, 3, 10],
+        "side": [4, 5, 6, 7],
+        "top": [8, 9],
+    }
+    for model in ("g20", "o30"):
+        suffix = "" if model == "g20" else "_o30"
+        tags = yaml.safe_load(
+            (
+                PACKAGE_ROOT
+                / "config"
+                / f"three_camera_tags{suffix}.yaml"
+            ).read_text()
+        )
+        all_ids = set()
+        for view, ids in expected.items():
+            key = f"/{model}_calibration/{view}/apriltag/apriltag"
+            parameters = tags[key]["ros__parameters"]
+            assert parameters["tag"]["ids"] == ids
+            assert parameters["size"] == 0.016
+            assert parameters["tag"]["sizes"] == [0.016] * len(ids)
+            assert parameters["qos_profile"] == "sensor_data"
+            assert parameters["detector"]["decimate"] == 1.5
+            all_ids.update(ids)
+        assert all_ids == set(range(11))
+        assert tags[
+            f"/{model}_calibration/side/apriltag/apriltag"
+        ]["ros__parameters"]["tag"]["frames"][0] == "side_base"
+
+
+def test_three_camera_launch_selects_o30_tag_parameters() -> None:
+    launch_text = (
+        PACKAGE_ROOT / "launch" / "three_camera_calibration.launch.py"
+    ).read_text()
+
+    assert 'tag_config = LaunchConfiguration("o30_tag_config")' in launch_text
+    assert '"three_camera_tags_o30.yaml"' in launch_text
+
+
+def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
+    )
+    parameters = config["/**"]["ros__parameters"]
+
+    assert parameters["tag_size_m"] == 0.016
+
+    assert parameters["baseline_command_u8"] == [
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        127,
+        127,
+        127,
+        127,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+    ]
+    assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd"
+    assert parameters["normal_calibration_speed"] == 15
+    assert parameters["index_roll_calibration_speed"] == 5
+    assert parameters["index_flex_calibration_speed"] == 10
+    assert parameters["o30_internal_speed_u8"] == 0
+    assert parameters["o30_command_full_range_seconds"] == 6.0
+    assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
+    assert parameters["speed_setting_settle_seconds"] >= 0.2
+    assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
+    assert parameters["pnp_group_initialization_frames"] == 8
+    assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
+    assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
+    assert parameters["repetitions"] == 3
+    assert parameters["validation_enabled"] is False
+    assert parameters["minimum_detection_rate"] == 0.95
+    assert parameters["minimum_detection_hz"] == 15.0
+    assert parameters["minimum_state_span_u8"] >= 240.0
+    assert parameters["endpoint_tolerance_u8"] == 2.0
+    assert parameters["o30_endpoint_tolerance_u8"] == 4.0
+    assert (
+        parameters["o30_thumb_cmc_roll_255_endpoint_tolerance_u8"] == 9.0
+    )
+    assert (
+        parameters["o30_index_mcp_roll_255_endpoint_tolerance_u8"] == 8.0
+    )
+    assert parameters["o30_thumb_mcp_zero_endpoint_tolerance_u8"] == 8.0
+    assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
+    assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
+    assert parameters["minimum_sweep_bins"] >= 32
+    assert parameters["maximum_bin_gap"] <= 16
+    assert parameters["automatic_sweep_retry_limit"] == 3
+    assert parameters["automatic_fit_retry_limit"] == 2
+    assert parameters["motor_stall_timeout_seconds"] >= 5.0
+    assert parameters["motor_stall_minimum_progress_u8"] == 1.0
+    assert parameters["automatic_motion_retry_limit"] == 2
+    assert parameters["provisional_warning_ratio"] == 1.25
+    assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
+    assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
+    assert parameters["position_timeout_seconds"] >= 20.0
+    assert parameters["maximum_state_image_skew_ms"] <= 50.0
+    assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
+    assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001
+    assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004
+    assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5
+    assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
+    assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
+    assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
+    assert parameters["zero_maximum_offset_deg"] <= 20.0
+    assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
+    assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
+    assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
+    assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
+    assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
+    assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
+    assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
+    assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
+    for view in ("front", "side", "top"):
+        assert parameters[f"{view}_camera_info_topic"].startswith(
+            f"/g20_calibration/{view}/"
+        )
+        assert parameters[f"{view}_detections_topic"] == (
+            f"/g20_calibration/{view}/apriltag/detections"
+        )
+
+
 def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
     config = yaml.safe_load(
         (PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
@@ -87,15 +253,6 @@ def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
     assert parameters["continuous_prepare_timeout_seconds"] >= 30.0
 
 
-def test_realsense_color_and_camera_info_use_matching_qos() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "realsense_color_qos.yaml").read_text()
-    )
-
-    assert config["color_qos"] == "DEFAULT"
-    assert config["color_info_qos"] == "DEFAULT"
-
-
 def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
     config = yaml.safe_load(
         (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text()

src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py

@@ -0,0 +1,326 @@
+"""Focused tests for checkerboard frame pairing."""
+
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node
+from g20_thumb_apriltag_calibration.extrinsics_node import (
+    BoardPose,
+    StereoCapture,
+    _fit_stereo_robust,
+    _individual_reprojection_passes,
+    _minimum_history_skew_ns,
+    _pose_is_novel,
+    _select_latest_synchronised_pair,
+    _summarize_transform_repeatability,
+)
+
+
+def _pose(stamp_ns: int) -> BoardPose:
+    return BoardPose(
+        stamp_ns=stamp_ns,
+        camera_from_board_candidates=(),
+        reprojection_rms_px=0.1,
+    )
+
+
+def test_pairing_uses_recent_history_instead_of_mismatched_latest_frames():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_030_000_000), _pose(1_370_000_000)]
+
+    pair = _select_latest_synchronised_pair(front, side, 100_000_000)
+
+    assert pair is not None
+    selected_front, selected_side, skew = pair
+    assert selected_front.stamp_ns == 1_000_000_000
+    assert selected_side.stamp_ns == 1_030_000_000
+    assert skew == 30_000_000
+
+
+def test_pairing_prefers_newest_valid_common_pair():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_010_000_000), _pose(1_240_000_000)]
+
+    pair = _select_latest_synchronised_pair(front, side, 100_000_000)
+
+    assert pair is not None
+    selected_front, selected_side, skew = pair
+    assert selected_front.stamp_ns == 1_200_000_000
+    assert selected_side.stamp_ns == 1_240_000_000
+    assert skew == 40_000_000
+
+
+def test_pairing_reports_minimum_skew_when_no_pair_passes():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_370_000_000)]
+
+    assert _select_latest_synchronised_pair(
+        front, side, 100_000_000
+    ) is None
+    assert _minimum_history_skew_ns(front, side) == 170_000_000
+
+
+def _transform(rotation_deg: float, translation_m: float) -> np.ndarray:
+    value = np.eye(4)
+    value[:3, :3] = Rotation.from_euler(
+        "z", rotation_deg, degrees=True
+    ).as_matrix()
+    value[0, 3] = translation_m
+    return value
+
+
+def test_repeatability_summary_accepts_consistent_capture_set():
+    captures = [
+        [_transform(-0.1, -0.0005)],
+        [_transform(0.0, 0.0)],
+        [_transform(0.1, 0.0005)],
+    ]
+
+    _, selected, rotation_error, translation_error = (
+        _summarize_transform_repeatability(captures)
+    )
+
+    assert len(selected) == 3
+    np.testing.assert_allclose(
+        np.degrees(rotation_error), 0.1, atol=1.0e-6
+    )
+    np.testing.assert_allclose(translation_error, 0.0005, atol=1.0e-9)
+
+
+def test_repeatability_summary_exposes_current_capture_outlier():
+    captures = [
+        [_transform(0.0, 0.0)],
+        [_transform(0.1, 0.0005)],
+        [_transform(1.0, 0.008)],
+    ]
+
+    _, _, rotation_error, translation_error = (
+        _summarize_transform_repeatability(captures)
+    )
+
+    assert np.degrees(rotation_error) > 0.3
+    assert translation_error > 0.0015
+
+
+def test_each_camera_must_pass_reprojection_gate_independently():
+    assert _individual_reprojection_passes(0.8, 1.1, 1.2)
+    assert not _individual_reprojection_passes(0.5, 1.4, 1.2)
+    assert _individual_reprojection_passes(0.5, 1.4, 1.5)
+    assert not _individual_reprojection_passes(0.5, 1.6, 1.5)
+    assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2)
+
+
+def test_pose_novelty_is_checked_against_all_previous_captures():
+    previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)]
+
+    assert not _pose_is_novel(
+        _transform(10.5, 0.105),
+        previous,
+        minimum_rotation_rad=np.deg2rad(2.0),
+        minimum_translation_m=0.015,
+    )
+    assert _pose_is_novel(
+        _transform(14.0, 0.13),
+        previous,
+        minimum_rotation_rad=np.deg2rad(2.0),
+        minimum_translation_m=0.015,
+    )
+
+
+def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate(
+    monkeypatch,
+):
+    captures = [
+        StereoCapture(
+            front_points_px=np.zeros((40, 2), dtype=np.float32),
+            other_points_px=np.zeros((40, 2), dtype=np.float32),
+            transform_candidates=(np.eye(4),),
+            pair_reprojection_rms_px=1.0,
+            skew_ns=1,
+        )
+        for _ in range(20)
+    ]
+
+    def fake_fit_once(
+        captures_arg,
+        indices,
+        object_points,
+        front_matrix,
+        other_matrix,
+        image_size,
+    ):
+        del (
+            captures_arg,
+            object_points,
+            front_matrix,
+            other_matrix,
+            image_size,
+        )
+        errors = np.asarray(
+            [1.1 if index < 15 else 1.5 for index in indices],
+            dtype=float,
+        )
+        return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors
+
+    monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
+    result = _fit_stereo_robust(
+        captures,
+        np.zeros((40, 3), dtype=np.float32),
+        np.eye(3),
+        np.eye(3),
+        (640, 480),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert result.passed
+    assert result.stereo_rms_px <= 1.2
+    assert len(result.inlier_indices) >= 15
+    assert result.rejected_indices
+    assert set(result.rejected_indices) <= set(range(15, 20))
+
+
+def test_joint_fit_reports_finite_provisional_metrics_before_minimum(
+    monkeypatch,
+):
+    captures = [
+        StereoCapture(
+            front_points_px=np.zeros((40, 2), dtype=np.float32),
+            other_points_px=np.zeros((40, 2), dtype=np.float32),
+            transform_candidates=(np.eye(4),),
+            pair_reprojection_rms_px=0.8,
+            skew_ns=1,
+        )
+        for _ in range(10)
+    ]
+
+    def fake_fit_once(
+        captures_arg,
+        indices,
+        object_points,
+        front_matrix,
+        other_matrix,
+        image_size,
+    ):
+        del (
+            captures_arg,
+            object_points,
+            front_matrix,
+            other_matrix,
+            image_size,
+        )
+        return 0.8, np.eye(4), np.full(len(indices), 0.8)
+
+    monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
+    result = _fit_stereo_robust(
+        captures,
+        np.zeros((40, 3), dtype=np.float32),
+        np.eye(3),
+        np.eye(3),
+        (640, 480),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert not result.passed
+    assert result.stereo_rms_px == 0.8
+    assert len(result.inlier_indices) == 10
+
+
+def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view():
+    random = np.random.default_rng(7)
+    object_points = np.zeros((40, 3), dtype=np.float32)
+    object_points[:, :2] = (
+        np.mgrid[0:8, 0:5].T.reshape(-1, 2) * 0.027
+    )
+    matrix = np.asarray(
+        [[1800.0, 0.0, 812.0], [0.0, 1795.0, 620.0], [0.0, 0.0, 1.0]]
+    )
+    other_from_front = np.eye(4)
+    other_from_front[:3, :3] = Rotation.from_euler(
+        "xyz", [2.0, 18.0, -1.0], degrees=True
+    ).as_matrix()
+    other_from_front[:3, 3] = [0.20, -0.01, 0.04]
+    expected_front_from_other = np.linalg.inv(other_from_front)
+    captures = []
+    for index in range(21):
+        front_from_board = np.eye(4)
+        front_from_board[:3, :3] = Rotation.from_euler(
+            "xyz",
+            [
+                -8.0 + index * 0.7,
+                5.0 + (index % 5) * 2.0,
+                -5.0 + (index % 4) * 3.0,
+            ],
+            degrees=True,
+        ).as_matrix()
+        front_from_board[:3, 3] = [
+            -0.08 + (index % 5) * 0.035,
+            -0.04 + (index % 4) * 0.025,
+            0.75 + (index % 3) * 0.08,
+        ]
+        other_from_board = other_from_front @ front_from_board
+        front_rvec = Rotation.from_matrix(
+            front_from_board[:3, :3]
+        ).as_rotvec()
+        other_rvec = Rotation.from_matrix(
+            other_from_board[:3, :3]
+        ).as_rotvec()
+        front_points, _ = cv2.projectPoints(
+            object_points,
+            front_rvec,
+            front_from_board[:3, 3],
+            matrix,
+            np.zeros(5),
+        )
+        other_points, _ = cv2.projectPoints(
+            object_points,
+            other_rvec,
+            other_from_board[:3, 3],
+            matrix,
+            np.zeros(5),
+        )
+        front_points = front_points.reshape(-1, 2)
+        other_points = other_points.reshape(-1, 2)
+        front_points += random.normal(0.0, 0.12, front_points.shape)
+        other_points += random.normal(0.0, 0.12, other_points.shape)
+        if index == 20:
+            other_points += random.normal(0.0, 4.0, other_points.shape)
+        captures.append(
+            StereoCapture(
+                front_points_px=front_points.astype(np.float32),
+                other_points_px=other_points.astype(np.float32),
+                transform_candidates=(expected_front_from_other.copy(),),
+                pair_reprojection_rms_px=0.2,
+                skew_ns=10_000_000,
+            )
+        )
+
+    result = _fit_stereo_robust(
+        captures,
+        object_points,
+        matrix,
+        matrix,
+        (1624, 1240),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert result.passed
+    assert 20 in result.rejected_indices
+    assert len(result.inlier_indices) >= 15
+    rotation_error = Rotation.from_matrix(
+        result.front_from_other[:3, :3]
+    ).inv() * Rotation.from_matrix(expected_front_from_other[:3, :3])
+    assert np.degrees(rotation_error.magnitude()) < 0.05
+    assert np.linalg.norm(
+        result.front_from_other[:3, 3]
+        - expected_front_from_other[:3, 3]
+    ) < 0.001

src/g20_thumb_apriltag_calibration/test/test_full_hand.py

@@ -0,0 +1,500 @@
+import math
+from dataclasses import replace
+
+import numpy as np
+import pytest
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    ACTIVE_JOINTS,
+    IMAGE_TRAJECTORY_JOINTS,
+    JOINT_SPECS,
+    LEFT_HAND_PROFILE,
+    MEASURED_JOINTS,
+    PASSIVE_JOINTS,
+    RIGHT_HAND_PROFILE,
+    SPLAY_JOINTS,
+    SWEEP_SPECS,
+    VIEW_TAGS,
+    build_calibration_motion_command,
+    build_calibration_speed_profile,
+    build_compact_payload,
+    build_full_hand_command,
+    center_splay_curve,
+    fit_joint_center_curve,
+    fit_joint_image_curve,
+    fit_measured_joint_curve,
+    fit_projected_zero,
+    get_hand_calibration_profile,
+    measure_joint_observation,
+    validate_compact_payload,
+)
+
+
+def _records() -> list[dict[str, object]]:
+    commands = list(range(0, 256, 16))
+    if commands[-1] != 255:
+        commands.append(255)
+    records: list[dict[str, object]] = []
+    centre = np.asarray([0.006, -0.004, 0.012])
+    radius = 0.025
+    image_centre = np.asarray([30.0, -12.0])
+    image_radius = 100.0
+    for cycle in range(3):
+        for direction, sequence in (
+            ("decreasing", reversed(commands)),
+            ("increasing", commands),
+        ):
+            for command in sequence:
+                angle = 0.70 * (255.0 - command) / 255.0
+                point = centre + np.asarray(
+                    [radius * math.cos(angle), radius * math.sin(angle), 0.0]
+                )
+                # At command 255 the inward vector points along image +x, so
+                # table_projected_zero_rad is exactly zero.
+                image_point = image_centre + np.asarray(
+                    [
+                        -image_radius * math.cos(angle),
+                        image_radius * math.sin(angle),
+                    ]
+                )
+                records.append(
+                    {
+                        "cycle": cycle,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_translation_xyz_m": point.tolist(),
+                        "image_relative_xy_px": image_point.tolist(),
+                    }
+                )
+    return records
+
+
+def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
+    assert len(JOINT_SPECS) == 21
+    assert len(ACTIVE_JOINTS) == 16
+    assert len(PASSIVE_JOINTS) == 5
+
+
+def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
+    profile = get_hand_calibration_profile("right")
+    assert profile is RIGHT_HAND_PROFILE
+    assert profile.reference_finger == "pinky"
+    assert [spec.motor_index for spec in profile.sweep_specs] == [
+        0, 5, 15, 9, 4, 19, 10
+    ]
+    assert profile.view_tags["front"]["pinky_roll"] == 10
+    assert profile.view_tags["side"] == {
+        "side_base": 4,
+        "pinky_mcp": 5,
+        "pinky_pip": 6,
+        "pinky_dip": 7,
+    }
+    assert profile.preflight_view_roles["side"] == (
+        "side_base",
+        "pinky_mcp",
+        "pinky_pip",
+        "pinky_dip",
+    )
+    thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
+    assert thumb_pitch.view == "front"
+    assert thumb_pitch.parent_role == "front_base"
+    assert thumb_pitch.child_role == "thumb_cmc"
+    assert profile.joint_specs["index_mcp_roll"].source_joint == (
+        "pinky_mcp_roll"
+    )
+    assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
+        "pinky_mcp_pitch"
+    )
+    assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
+    assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
+
+    roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
+    command = build_calibration_motion_command(
+        roll, 127, profile=profile
+    )
+    assert command[9] == 127
+    assert command[6:9] == [255, 255, 255]
+    speeds = build_calibration_speed_profile(
+        roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    )
+    assert speeds == [15, 15, 15, 15, 5]
+    assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
+        range(11)
+    )
+    assert VIEW_TAGS["front"]["index_roll"] == 10
+    assert VIEW_TAGS["side"] == {
+        "side_base": 4,
+        "index_mcp": 5,
+        "index_pip": 6,
+        "index_dip": 7,
+    }
+    assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
+    assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
+
+
+def test_joint_trajectory_spaces_match_observation_geometry() -> None:
+    assert IMAGE_TRAJECTORY_JOINTS == {
+        "thumb_cmc_pitch",
+        "thumb_cmc_roll",
+        "thumb_mcp",
+        "thumb_ip",
+        "index_mcp_roll",
+        "index_mcp_pitch",
+        "index_pip",
+    }
+    records = _records()
+    for name in IMAGE_TRAJECTORY_JOINTS:
+        fit = fit_measured_joint_curve(name, records)
+        assert fit.circle["space"] == "image_2d"
+    for name in ("index_dip", "thumb_cmc_yaw"):
+        assert "space" not in fit_measured_joint_curve(name, records).circle
+
+
+def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
+    result = build_full_hand_command(6, 27)
+    assert result[6] == 27
+    assert result[:6] == [255] * 6
+    with pytest.raises(ValueError, match="controlled"):
+        build_full_hand_command(11, 27)
+
+
+def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+
+    result = build_calibration_motion_command(index_roll, 27, baseline)
+
+    assert result[6:10] == [27, 0, 0, 0]
+    assert result[:6] == baseline[:6]
+    assert result[10:] == baseline[10:]
+
+
+def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
+    profile = RIGHT_HAND_PROFILE
+    pinky_roll = next(
+        spec for spec in profile.sweep_specs if spec.motor_index == 9
+    )
+
+    result = build_calibration_motion_command(
+        pinky_roll, 27, profile=profile
+    )
+
+    assert result[6:10] == [255, 255, 255, 27]
+    assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
+
+
+def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+
+    result = build_calibration_motion_command(thumb_pitch, 17, baseline)
+
+    assert result[0] == 17
+    assert result[6:10] == [127, 127, 127, 127]
+
+
+def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
+    profile = RIGHT_HAND_PROFILE
+    thumb_pitch = next(
+        spec for spec in profile.sweep_specs if spec.motor_index == 0
+    )
+
+    result = build_calibration_motion_command(
+        thumb_pitch, 17, profile=profile
+    )
+
+    assert result[0] == 17
+    assert result[10] == 255
+    assert result[5] == 255
+    assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
+
+
+def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+
+    result = build_calibration_motion_command(thumb_pitch, 17)
+
+    assert result[0] == 17
+    assert result[5] == 255
+    assert result[10] == 255
+    assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
+
+
+def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
+
+    result = build_calibration_motion_command(thumb_yaw, 27, baseline)
+
+    assert result[5] == 145
+    assert result[10] == 27
+    assert result[:5] == baseline[:5]
+    assert result[6:10] == baseline[6:10]
+    assert result[11:] == baseline[11:]
+
+
+def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+    index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
+
+    assert build_calibration_speed_profile(
+        index_roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 5, 15, 15, 15]
+    assert build_calibration_speed_profile(
+        index_pitch,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 10, 15, 15, 15]
+
+    index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
+    assert build_calibration_speed_profile(
+        index_pip,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 10, 15, 15, 15]
+
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+    assert build_calibration_speed_profile(
+        thumb_pitch,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 15, 15, 15, 15]
+
+
+def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+    with pytest.raises(ValueError, match="speeds"):
+        build_calibration_speed_profile(
+            index_roll,
+            normal_speed=15,
+            index_roll_speed=256,
+            index_flex_speed=10,
+        )
+
+
+def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
+    records = _records()
+    fit = fit_joint_center_curve(records)
+    assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
+    assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
+    assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
+    assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
+
+
+def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
+    records = _records()
+    for record in records:
+        command = int(record["command_u8"])
+        record["relative_translation_xyz_m"] = [
+            0.001 * command,
+            0.0,
+            0.0,
+        ]
+
+    fit = fit_measured_joint_curve("thumb_mcp", records)
+
+    assert fit.circle["space"] == "image_2d"
+    assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
+    assert fit.angle_rad[255] == pytest.approx(0.0)
+    assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
+    assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
+
+    command_zero = next(
+        record for record in records if int(record["command_u8"]) == 0
+    )
+    observed = measure_joint_observation(
+        fit,
+        vector_xyz_m=command_zero["relative_translation_xyz_m"],
+        image_vector_xy_px=command_zero["image_relative_xy_px"],
+    )
+    assert observed == pytest.approx(0.70, abs=0.02)
+
+
+def test_image_curve_rejects_insufficient_projected_arc() -> None:
+    records = _records()
+    for record in records:
+        command = float(record["command_u8"])
+        angle = math.radians(2.0) * (255.0 - command) / 255.0
+        record["image_relative_xy_px"] = [
+            100.0 * math.cos(angle),
+            100.0 * math.sin(angle),
+        ]
+
+    with pytest.raises(
+        ValueError, match="joint_image_trajectory_quality_failed:arc"
+    ):
+        fit_joint_image_curve(records)
+
+
+def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
+    fit = fit_joint_center_curve(_records())
+    centred, zero_command, midpoint = center_splay_curve(fit)
+    assert midpoint == pytest.approx(0.35, abs=1.0e-4)
+    assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
+    assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
+    assert zero_command in {127, 128}
+    assert abs(centred.angle_rad[zero_command]) <= 0.002
+
+
+def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "index_mcp_roll" else base
+        for name in MEASURED_JOINTS
+    }
+    offsets = {name: 0.01 for name in ACTIVE_JOINTS}
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+    payload = build_compact_payload(
+        serial_number="G20_LEFT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.01, -0.02],
+        passed=True,
+        baseline=baseline,
+    )
+    validate_compact_payload(payload)
+    assert set(payload) == {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "angle_unit",
+        "command_range",
+        "baseline_command_u8",
+        "joints",
+        "quality",
+    }
+    assert len(payload["joints"]) == 21
+    assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
+    assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
+    assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
+    assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
+    assert (
+        payload["joints"]["middle_mcp_roll"]["angle_rad"]
+        == payload["joints"]["index_mcp_roll"]["angle_rad"]
+    )
+    assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
+        "urdf_zero_offset_rad": pytest.approx(0.01)
+    }
+    for name in SPLAY_JOINTS:
+        assert payload["joints"][name]["zero_command_u8"] == zero_command
+
+
+def test_compact_payload_allows_skipped_random_validation() -> None:
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "index_mcp_roll" else base
+        for name in MEASURED_JOINTS
+    }
+    offsets = {name: 0.0 for name in ACTIVE_JOINTS}
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+
+    payload = build_compact_payload(
+        serial_number="G20_LEFT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[],
+        passed=True,
+        baseline=baseline,
+    )
+
+    validate_compact_payload(payload)
+    assert payload["quality"] == {
+        "passed": True,
+        "validation_mae_rad": None,
+        "validation_p95_rad": None,
+    }
+
+
+def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
+    profile = RIGHT_HAND_PROFILE
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "pinky_mcp_roll" else base
+        for name in profile.measured_joints
+    }
+    offsets = {
+        name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
+        for name in profile.active_joints
+    }
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+    payload = build_compact_payload(
+        serial_number="G20_RIGHT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.01],
+        passed=True,
+        baseline=baseline,
+        side="right",
+    )
+
+    validate_compact_payload(payload)
+    assert payload["schema_version"] == 4
+    assert payload["side"] == "right"
+    assert set(payload) == {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "angle_unit",
+        "command_range",
+        "baseline_command_u8",
+        "joints",
+        "quality",
+    }
+    for finger in ("index", "middle", "ring"):
+        assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
+            "pinky_mcp_roll"
+        )
+        assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
+            "pinky_mcp_pitch"
+        )
+        assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
+            "pinky_pip"
+        )
+        for suffix in ("mcp_roll", "mcp_pitch", "pip"):
+            joint = payload["joints"][f"{finger}_{suffix}"]
+            source = payload["joints"][joint["source_joint"]]
+            assert joint["angle_rad"] == source["angle_rad"]
+            assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
+            assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}

src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py

@@ -0,0 +1,104 @@
+from pathlib import Path
+
+import pytest
+import yaml
+
+from g20_thumb_apriltag_calibration.hikrobot_camera import (
+    DeviceDescriptor,
+    decode_c_string,
+    load_camera_calibration,
+    resolve_camera_info_path,
+    select_device,
+)
+
+
+def test_decode_c_string_stops_at_first_null() -> None:
+    assert decode_c_string(b"DB2163742\0ignored") == "DB2163742"
+
+
+def test_select_device_accepts_serial_or_guid() -> None:
+    devices = [
+        DeviceDescriptor(
+            index=0,
+            model="MV-CS020-10UM",
+            serial="DB2163742",
+            guid="2BDFB2163742",
+        ),
+        DeviceDescriptor(
+            index=1,
+            model="MV-CS020-10UM",
+            serial="DB2163739",
+            guid="2BDFB2163739",
+        ),
+    ]
+
+    assert select_device(devices, "DB2163742", "MV-CS020-10UM").index == 0
+    assert select_device(devices, "2BDFB2163739", "MV-CS020-10UM").index == 1
+
+
+def test_select_device_never_guesses_when_multiple_cameras_exist() -> None:
+    devices = [
+        DeviceDescriptor(0, "MV-CS020-10UM", "one", "guid-one"),
+        DeviceDescriptor(1, "MV-CS020-10UM", "two", "guid-two"),
+    ]
+
+    with pytest.raises(RuntimeError, match="selector is required"):
+        select_device(devices, "", "MV-CS020-10UM")
+
+
+def test_select_device_rejects_wrong_model() -> None:
+    devices = [DeviceDescriptor(0, "other", "DB2163742", "guid")]
+
+    with pytest.raises(RuntimeError, match="expected a model"):
+        select_device(devices, "DB2163742", "MV-CS020-10UM")
+
+
+def test_load_standard_camera_calibration(tmp_path: Path) -> None:
+    path = tmp_path / "camera.yaml"
+    path.write_text(
+        yaml.safe_dump(
+            {
+                "image_width": 1624,
+                "image_height": 1240,
+                "camera_name": "hikrobot_front_DB2163742",
+                "camera_matrix": {
+                    "rows": 3,
+                    "cols": 3,
+                    "data": [1000.0, 0.0, 812.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 1.0],
+                },
+                "distortion_model": "plumb_bob",
+                "distortion_coefficients": {
+                    "rows": 1,
+                    "cols": 5,
+                    "data": [0.1, -0.2, 0.0, 0.0, 0.1],
+                },
+                "rectification_matrix": {
+                    "rows": 3,
+                    "cols": 3,
+                    "data": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+                },
+                "projection_matrix": {
+                    "rows": 3,
+                    "cols": 4,
+                    "data": [1000.0, 0.0, 812.0, 0.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 0.0, 1.0, 0.0],
+                },
+            },
+            sort_keys=False,
+        ),
+        encoding="utf-8",
+    )
+
+    calibration = load_camera_calibration(path)
+
+    assert calibration.width == 1624
+    assert calibration.height == 1240
+    assert calibration.k[0] == 1000.0
+    assert calibration.p[5] == 1001.0
+
+
+def test_camera_info_url_only_accepts_local_files(tmp_path: Path) -> None:
+    path = resolve_camera_info_path(str(tmp_path / "front.yaml"))
+    assert path == (tmp_path / "front.yaml").resolve()
+    assert resolve_camera_info_path("") is None
+    with pytest.raises(ValueError, match="filesystem path"):
+        resolve_camera_info_path("package://example/front.yaml")

src/g20_thumb_apriltag_calibration/test/test_o30_support.py

@@ -0,0 +1,263 @@
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    O30_COMMAND_NAMES,
+    O30_RIGHT_BASELINE_COMMAND,
+    O30_RIGHT_HAND_PROFILE,
+    JointCurveFit,
+    build_calibration_motion_command,
+    build_calibration_speed_profile,
+    build_compact_payload,
+    get_hand_calibration_profile,
+    validate_compact_payload,
+)
+from g20_thumb_apriltag_calibration.urdf_zero import (
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    get_zero_calibration_profile,
+    solve_urdf_zero_offsets,
+    write_zero_corrected_urdf,
+)
+
+
+O30_SOURCE_URDF = Path(
+    "/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/"
+    "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
+)
+
+
+def _curve(zero_command: int, travel_rad: float = 0.8) -> JointCurveFit:
+    values = np.asarray(
+        [travel_rad * (command - zero_command) / 255.0 for command in range(256)]
+    )
+    data = tuple(float(value) for value in values)
+    return JointCurveFit(
+        angle_rad=data,
+        decreasing_rad=data,
+        increasing_rad=data,
+        circle={},
+        maximum_monotonic_correction_rad=0.0,
+        maximum_hysteresis_rad=0.0,
+        quality={},
+    )
+
+
+def test_o30_right_profile_matches_sdk_and_requested_baseline() -> None:
+    profile = get_hand_calibration_profile("right", "O30")
+
+    assert profile is O30_RIGHT_HAND_PROFILE
+    assert profile.command_names == O30_COMMAND_NAMES
+    assert profile.baseline_command == (
+        0, 0, 255, 205, 165, 20,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    )
+    assert O30_RIGHT_BASELINE_COMMAND == profile.baseline_command
+    assert len(profile.active_joints) == 20
+    assert profile.passive_joints == ()
+    assert set(profile.measured_joints) == {
+        "thumb_cmc_roll",
+        "thumb_cmc_yaw",
+        "thumb_mcp",
+        "thumb_ip",
+        "pinky_mcp_roll",
+        "pinky_mcp_pitch",
+        "pinky_pip",
+        "pinky_dip",
+    }
+    assert [spec.motor_index for spec in profile.sweep_specs] == [
+        0, 6, 15, 5, 10, 14, 19, 1,
+    ]
+    assert profile.joint_specs["index_mcp_roll"].motor_index == 2
+    assert profile.joint_specs["pinky_mcp_roll"].motor_index == 5
+    assert profile.joint_specs["thumb_mcp"].motor_index == 6
+    assert profile.joint_specs["pinky_mcp_pitch"].motor_index == 10
+    assert profile.joint_specs["pinky_pip"].motor_index == 14
+    assert profile.joint_specs["thumb_ip"].motor_index == 15
+    assert profile.joint_specs["pinky_dip"].motor_index == 19
+
+    with pytest.raises(ValueError, match="only the right hand"):
+        get_hand_calibration_profile("left", "O30")
+
+
+def test_o30_pinky_roll_clearance_and_sdk_speed_broadcast() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 5)
+    command = build_calibration_motion_command(roll, 100, profile=profile)
+
+    assert command[0:6] == [0, 0, 255, 255, 255, 100]
+    assert command[6:] == [0] * 14
+    assert build_calibration_speed_profile(
+        roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    ) == [5] * 5
+
+    dip = next(spec for spec in profile.sweep_specs if spec.motor_index == 19)
+    assert build_calibration_speed_profile(
+        dip,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    ) == [10] * 5
+
+
+def test_o30_static_zero_policy_and_compact_payload() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    zero = get_zero_calibration_profile("right", "O30")
+
+    assert set(zero.fixed_direct_zero_offsets_rad) == {
+        "thumb_ip",
+        "pinky_mcp_roll",
+        "pinky_dip",
+    }
+    assert "pinky_mcp_pitch" not in zero.fixed_direct_zero_offsets_rad
+    assert "pinky_pip" not in zero.fixed_direct_zero_offsets_rad
+    assert zero.inherited_static_zero_joints == {}
+    assert zero.inherited_zero_joints["index_dip"] == "pinky_dip"
+
+    measured = {
+        name: _curve(
+            profile.baseline_command[profile.joint_specs[name].motor_index]
+        )
+        for name in profile.measured_joints
+    }
+    offsets = {name: 0.0 for name in profile.active_joints}
+    offsets["pinky_mcp_pitch"] = 0.01
+    offsets["pinky_pip"] = -0.02
+    payload = build_compact_payload(
+        serial_number="O30_RIGHT_TEST",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.001],
+        passed=True,
+        side="right",
+        model="O30",
+    )
+
+    validate_compact_payload(payload)
+    assert payload["model"] == "O30"
+    assert len(payload["joints"]) == 20
+    for name in (
+        "index_mcp_roll",
+        "middle_mcp_roll",
+        "ring_mcp_roll",
+        "pinky_mcp_roll",
+    ):
+        joint = payload["joints"][name]
+        assert joint["angle_rad"][joint["zero_command_u8"]] == pytest.approx(0.0)
+        assert joint["zero_angles"]["urdf_zero_offset_rad"] == 0.0
+    assert payload["joints"]["pinky_mcp_pitch"]["zero_angles"] == {
+        "urdf_zero_offset_rad": 0.01
+    }
+    assert payload["joints"]["index_mcp_pitch"]["zero_angles"] == {
+        "urdf_zero_offset_rad": 0.0
+    }
+
+
+@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
+def test_o30_source_urdf_has_exact_active_joint_set_and_writer(tmp_path) -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    source_bytes = O30_SOURCE_URDF.read_bytes()
+    model = UrdfKinematicModel(O30_SOURCE_URDF)
+
+    assert set(model.joints) == set(profile.active_joints)
+    assert "thumb_cmc_pitch" not in model.joints
+    destination = write_zero_corrected_urdf(
+        source_urdf=O30_SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="O30_RIGHT_TEST",
+        offsets_rad={name: 0.0 for name in profile.active_joints},
+        timestamp="20260814_120000",
+    )
+    assert destination.is_file()
+    assert O30_SOURCE_URDF.read_bytes() == source_bytes
+    assert set(UrdfKinematicModel(destination).joints) == set(profile.active_joints)
+
+
+@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
+def test_o30_zero_solver_recovers_only_observable_offsets() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    zero = get_zero_calibration_profile("right", "O30")
+    curves = {
+        name: _curve(
+            profile.baseline_command[profile.joint_specs[name].motor_index],
+            math.radians(55.0),
+        )
+        for name in profile.measured_joints
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in profile.joint_specs.items()
+    }
+    expected_degrees = {
+        "thumb_cmc_roll": 2.0,
+        "thumb_cmc_yaw": -3.0,
+        "thumb_mcp": 4.0,
+        "thumb_ip": 0.0,
+        "pinky_mcp_roll": 0.0,
+        "pinky_mcp_pitch": 1.2,
+        "pinky_pip": -1.0,
+        "pinky_dip": 0.0,
+    }
+    offsets = {
+        name: math.radians(expected_degrees[name])
+        for name in zero.direct_zero_joints
+    }
+    model = UrdfKinematicModel(O30_SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
+    base_translation = np.asarray([0.25, -0.12, 0.68])
+    measurements = []
+    for cycle in range(3):
+        for joint in zero.axis_joints:
+            state = tuple(float(value) for value in profile.baseline_command)
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            axis, point = model.axis_line(
+                joint,
+                zero_offsets=offsets,
+                joint_angles=angles,
+            )
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=state,
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                    pose_axis_line_rms_m=0.0001,
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=O30_SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type="right",
+        hand_model="O30",
+    )
+
+    assert result.passed is True
+    for name, expected in expected_degrees.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            expected, abs=0.05
+        )
+    assert result.all_active_offsets_rad["index_mcp_roll"] == 0.0
+    assert result.all_active_offsets_rad["index_mcp_pitch"] == 0.0

src/g20_thumb_apriltag_calibration/test/test_offline_replay.py

@@ -0,0 +1,44 @@
+import pytest
+
+from g20_thumb_apriltag_calibration.offline_replay import (
+    _latest_attempt_records,
+    _output_suffix,
+)
+
+
+def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
+    return {
+        "kind": "sample",
+        "joint": joint,
+        "cycle": cycle,
+        "direction": direction,
+        "attempt": attempt,
+    }
+
+
+def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
+    rows = [
+        {"kind": "session_start"},
+        _sample("pinky_pip", 0, "decreasing", 1),
+        _sample("pinky_pip", 0, "decreasing", 3),
+        _sample("pinky_pip", 0, "increasing", 1),
+        _sample("pinky_pip", 1, "decreasing", 2),
+        _sample("thumb_cmc_yaw", 0, "decreasing", 1),
+    ]
+
+    selected = _latest_attempt_records(rows)
+
+    assert [
+        record["attempt"] for record in selected["pinky_pip"]
+    ] == [3, 1, 2]
+    assert [
+        record["attempt"] for record in selected["thumb_cmc_yaw"]
+    ] == [1]
+
+
+def test_output_suffix_is_safe_and_explicit() -> None:
+    assert _output_suffix(None) == ""
+    assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
+    for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
+        with pytest.raises(ValueError, match="output tag"):
+            _output_suffix(invalid)

src/g20_thumb_apriltag_calibration/test/test_pnp.py

@@ -324,6 +324,125 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
     assert selected == {"t4": return_t4, "t5": return_t5}
 
 
+def test_group_tracker_initializes_from_multiple_static_frames() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("parent", "child"),
+        adjacent_pairs=(("parent", "child"),),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+    )
+    parent = _pose(0.0, 0.00, 0.05)
+    true_child = _pose(20.0, 0.03, 0.10)
+
+    for index in range(7):
+        false_child = _pose(5.0 * index, 0.03, 0.05)
+        selected, reason = tracker.select(
+            {
+                "parent": (parent,),
+                "child": (false_child, true_child),
+            },
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+        assert selected is None
+        assert reason == f"group_initializing:{index + 1}/8"
+
+    false_child = _pose(35.0, 0.03, 0.05)
+    selected, reason = tracker.select(
+        {
+            "parent": (parent,),
+            "child": (false_child, true_child),
+        },
+        stamp_ns=1_231_000_000,
+    )
+
+    assert reason == ""
+    assert selected == {"parent": parent, "child": true_child}
+    assert (
+        tracker.last_initialization_quality["initialization_search"]
+        == "static_reference"
+    )
+    assert tracker.last_initialization_quality[
+        "p95_pair_rotation_drift_rad"
+    ] < np.deg2rad(1.0)
+
+
+def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("mcp", "pip", "dip"),
+        adjacent_pairs=(("mcp", "pip"), ("pip", "dip")),
+        normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")),
+        normal_alignment_scale_rad=np.deg2rad(5.0),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+        maximum_normal_alignment_rad=np.deg2rad(15.0),
+    )
+    mcp = _pose(12.0, 0.00, 0.05)
+    correct_pip = _pose(14.0, 0.03, 0.10)
+    mirror_pip = _pose(50.0, 0.03, 0.05)
+    dip = _pose(15.0, 0.06, 0.05)
+
+    for index in range(8):
+        selected, reason = tracker.select(
+            {
+                "mcp": (mcp,),
+                "pip": (mirror_pip, correct_pip),
+                "dip": (dip,),
+            },
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+
+    assert reason == ""
+    assert selected == {
+        "mcp": mcp,
+        "pip": correct_pip,
+        "dip": dip,
+    }
+    assert tracker.last_initialization_quality[
+        "maximum_normal_alignment_rad"
+    ] < np.deg2rad(3.0)
+
+
+def test_static_group_rejects_when_no_aligned_branch_exists() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("base", "moving"),
+        adjacent_pairs=(("base", "moving"),),
+        normal_alignment_pairs=(("base", "moving"),),
+        normal_alignment_scale_rad=np.deg2rad(5.0),
+        maximum_normal_alignment_rad=np.deg2rad(15.0),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+    )
+    base = _pose(0.0, 0.00, 0.05)
+    mirror = _pose(38.0, 0.03, 0.05)
+
+    for index in range(8):
+        selected, reason = tracker.select(
+            {"base": (base,), "moving": (mirror,)},
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+
+    assert selected is None
+    assert reason == "group_normal_alignment"
+
+
 def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None:
     roles = ("t3", "t4", "t5")
     mount_rotations = {

src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py

@@ -0,0 +1,406 @@
+import json
+
+import numpy as np
+
+from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
+    _task_text,
+    render_three_camera_status_text_zh,
+    three_camera_reason_zh,
+)
+
+
+def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "sweep_missing_endpoint_bin",
+        "progress": 0.1429,
+        "completed_sweeps": 6,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "sweep",
+            "view": "front",
+            "motor_index": 5,
+            "joints": ["thumb_cmc_roll"],
+            "cycle": 1,
+            "repetitions": 3,
+            "start_u8": 255,
+            "target_u8": 0,
+            "actual_u8": 0.4,
+            "motion_progress": 0.998,
+            "valid_frames": 239,
+            "sample": {
+                "minimum_u8": 0.4,
+                "maximum_u8": 248.2,
+                "bin_count": 180,
+                "minimum_bin_count": 32,
+                "maximum_bin_gap": 3,
+                "allowed_maximum_bin_gap": 16,
+                "missing_endpoint_u8": [255],
+                "endpoint_tolerance_u8": 2.0,
+            },
+        },
+        "views": {
+            "front": {
+                "ready": False,
+                "detection_hz": 30.04,
+                "valid_rate": 0.0,
+                "missing_tag_ids": [2],
+                "pnp_rejections": {},
+                "group_pnp_reason": "group_pose_jump",
+                "pnp_invalid_seconds": 0.8,
+                "pnp_reset_count": 2,
+            },
+            "side": {
+                "ready": True,
+                "detection_hz": 30.02,
+                "valid_rate": 1.0,
+                "missing_tag_ids": [],
+            },
+        },
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "标定已暂停（PAUSED）" in text
+    assert "缺少电机端点255" in text
+    assert "实际电机范围为0.4～248.2" in text
+    assert "第1/3轮，255→0" in text
+    assert "计划扫描6/42个方向，扫描进度14.3%" in text
+    assert "当前缺失Tag=2" in text
+    assert "group_pose_jump" not in text
+    assert "PnP拒绝" not in text
+    assert "自动重置" not in text
+    assert "/g20_calibration/resume" in text
+
+
+def test_preflight_lists_missing_tags_in_chinese() -> None:
+    payload = {
+        "state": "PREFLIGHT",
+        "reason": "waiting_for_three_cameras_tags_and_sdk",
+        "progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 0,
+        "active": {},
+        "views": {
+            "top": {
+                "ready": False,
+                "detection_hz": 30.0,
+                "valid_rate": 0.0,
+                "missing_tag_ids": [9],
+            }
+        },
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "设备和标签预检（PREFLIGHT）" in text
+    assert "当前缺失Tag=9" in text
+    assert "全部必需Tag同时有效率0.0%" in text
+
+
+def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "joint_fit_check_failed",
+        "progress": 18 / 42,
+        "completed_sweeps": 18,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "fit_failure",
+            "view": "front",
+            "motor_index": 6,
+            "joints": ["index_mcp_roll"],
+            "attempt": 1,
+            "directions_to_rescan": 6,
+            "failures": [
+                {
+                    "joint": "index_mcp_roll",
+                    "metric": "arc_deg",
+                    "actual": 3.98,
+                    "limit": 15.0,
+                    "comparison": "minimum",
+                }
+            ],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "食指MCP侧摆的实测圆弧为3.98°" in text
+    assert "要求至少15.00°" in text
+    assert "只清除当前失败关节的数据并重扫6个方向" in text
+    assert "第1次尝试" in text
+    assert "运动采样：" not in text
+
+
+def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "zero_model_validation_failed",
+        "progress": 0.92,
+        "scan_progress": 1.0,
+        "completed_sweeps": 42,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "zero_model_failure",
+            "view": "front",
+            "motor_index": 15,
+            "joints": ["thumb_mcp", "thumb_ip"],
+            "directions_to_rescan": 0,
+            "failures": [
+                {
+                    "joint": "thumb_mcp",
+                    "metric": "zero_guard",
+                    "reason": "zero_offset_exceeds_configured_limit",
+                    "actual_deg": -39.81,
+                    "limit_deg": 20.0,
+                }
+            ],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "拇指MCP：零位估计超过安全范围" in text
+    assert "估计-39.81°，允许±20.00°" in text
+    assert "不会自动重扫" in text
+    assert "总体进度：92.0%" in text
+    assert "扫描进度100.0%" in text
+    assert "运动采样：" not in text
+
+
+def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
+    active = {
+        "kind": "sweep",
+        "view": "front",
+        "motor_index": 15,
+        "joints": ["thumb_mcp", "thumb_ip"],
+        "cycle": 1,
+        "repetitions": 3,
+        "start_u8": 255,
+        "target_u8": 0,
+        "fit_attempt": 2,
+        "fit_attempt_limit": 3,
+    }
+
+    assert "整关节自动重采第2/3次" in _task_text(active)
+
+
+def test_o30_status_shows_cycle_order_retry_execution_and_namespace() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "joint_fit_check_failed",
+        "progress": 0.112,
+        "scan_progress": 0.125,
+        "completed_sweeps": 6,
+        "total_sweeps": 48,
+        "executed_sweep_directions": 20,
+        "service_prefix": "/o30_calibration",
+        "active": {
+            "kind": "fit_failure",
+            "view": "front",
+            "motor_index": 0,
+            "joints": ["thumb_cmc_roll"],
+            "attempt": 3,
+            "fit_attempt_limit": 3,
+            "directions_to_rescan": 6,
+            "failures": [],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "已启动20个方向（含自动重扫）" in text
+    assert "重扫不会重复增加计划进度" in text
+    assert "当前为第3/3次采集结果" in text
+    assert "/o30_calibration/resume" in text
+    assert "/g20_calibration/resume" not in text
+
+
+def test_motor_stall_reason_is_explained_in_chinese() -> None:
+    reason, action = three_camera_reason_zh(
+        "PAUSED",
+        "motor_state_stalled:sweep_motor_19:error_u8=5.000",
+        {},
+    )
+
+    assert "距目标5.000个u8" in reason
+    assert "机械端点" in action
+    assert "不要反复调用resume" in action
+
+
+def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": (
+            "motor_state_stalled:return_baseline:motor_index=10:"
+            "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
+            "error_u8=5.000"
+        ),
+        "progress": 0.0,
+        "scan_progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "motion_stall",
+            "stage": "return_baseline",
+            "motor_index": 10,
+            "target_u8": 255.0,
+            "actual_u8": 250.0,
+            "error_u8": 5.0,
+            "tolerance_u8": 4.0,
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "电机10反馈连续8秒" in text
+    assert "目标255.0、实际250.0、误差5.000 u8" in text
+    assert "允许容差±4.0 u8" in text
+    assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text
+    assert "运动采样：" not in text
+
+
+def test_return_baseline_prints_the_exact_command() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    payload = {
+        "state": "RETURN_BASELINE",
+        "reason": "return_baseline_before_next_sweep",
+        "progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 42,
+        "baseline_command_u8": baseline,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "正在恢复目标姿态（RETURN_BASELINE）" in text
+    assert f"正在确认基准姿态：{baseline}" in text
+
+
+def test_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    recovery = list(baseline)
+    recovery[5] = 145
+    recovery[10] = 0
+    payload = {
+        "state": "RETURN_BASELINE",
+        "reason": "return_baseline_before_retry_sweep",
+        "progress": 0.5,
+        "completed_sweeps": 36,
+        "total_sweeps": 42,
+        "baseline_command_u8": baseline,
+        "return_command_u8": recovery,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert f"正在确认恢复姿态：{recovery}" in text
+
+
+def test_status_numeric_diagnostics_are_json_serializable() -> None:
+    bins = [0, 16, 255]
+    payload = {
+        "state": "SWEEP",
+        "reason": "collecting_timestamp_synchronised_tag_centres",
+        "active": {
+            "sample": {
+                # np.diff返回NumPy标量；节点必须在放入状态前转成原生int。
+                "maximum_bin_gap": int(max(np.diff(bins), default=0)),
+            }
+        },
+    }
+
+    encoded = json.dumps(payload, ensure_ascii=False)
+
+    assert '"maximum_bin_gap": 239' in encoded
+
+
+def test_urdf_zero_bound_has_specific_chinese_scale_guidance() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": (
+            "URDF zero offset reached the configured 20.000 degree bound: "
+            "thumb_cmc_roll=+20.000deg, index_pip=-20.000deg; "
+            "all_offsets: thumb_cmc_roll=+20.000deg"
+        ),
+        "progress": 1.0,
+        "completed_sweeps": 42,
+        "total_sweeps": 42,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "触及±20.000°安全边界" in text
+    assert "拇指CMC滚转=+20.000deg" in text
+    assert "食指PIP=-20.000deg" in text
+    assert "不要调用resume" in text
+    assert "Tag有效黑框边长" in text
+    assert "未分类原因码" not in text
+
+
+def test_index_roll_status_prints_clearance_motor_feedback() -> None:
+    payload = {
+        "state": "SWEEP",
+        "reason": "collecting_timestamp_synchronised_tag_centres",
+        "progress": 0.43,
+        "completed_sweeps": 18,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "sweep",
+            "view": "front",
+            "motor_index": 6,
+            "joints": ["index_mcp_roll"],
+            "cycle": 1,
+            "repetitions": 3,
+            "automatic_retry_count": 1,
+            "automatic_retry_limit": 2,
+            "start_u8": 255,
+            "target_u8": 0,
+            "actual_u8": 44.0,
+            "motion_progress": 0.827,
+            "valid_frames": 971,
+            "sample": {"minimum_u8": 40.0, "maximum_u8": 253.0},
+            "auxiliary_motors": [
+                {"motor_index": 7, "command_u8": 0, "actual_u8": 0.0},
+                {"motor_index": 8, "command_u8": 0, "actual_u8": 1.0},
+                {"motor_index": 9, "command_u8": 0, "actual_u8": 0.0},
+            ],
+            "speed": {
+                "commanded_finger_speed": [15, 5, 15, 15, 15],
+                "reported_finger_speed": [15, 5, 15, 15, 15],
+            },
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "避挡姿态：电机7目标0、实际0.0" in text
+    assert "电机8目标0、实际1.0" in text
+    assert "电机9目标0、实际0.0" in text
+    assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
+    assert "SDK报告[15, 5, 15, 15, 15]" in text
+    assert "自动重试：当前方向已自动重扫1/2次" in text

src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py

@@ -0,0 +1,977 @@
+import math
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.extrinsics import (
+    camera_info_fingerprint,
+    dump_three_camera_extrinsics,
+    load_three_camera_extrinsics,
+)
+from g20_thumb_apriltag_calibration.urdf_zero import (
+    AXIS_JOINTS,
+    DIRECT_ZERO_JOINTS,
+    INHERITED_ZERO_JOINTS,
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    _zero_sensitive_axis_error_rad,
+    fit_joint_axis_measurement,
+    fit_rotation_joint_curve,
+    solve_urdf_zero_offsets,
+    get_zero_calibration_profile,
+    write_zero_corrected_urdf,
+)
+from g20_thumb_apriltag_calibration.full_hand import (
+    ACTIVE_JOINTS,
+    JOINT_SPECS,
+    MEASURED_JOINTS,
+    PASSIVE_JOINTS,
+    JointCurveFit,
+    get_hand_calibration_profile,
+)
+
+
+REPOSITORY = Path(__file__).resolve().parents[3]
+SOURCE_URDF = REPOSITORY / (
+    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+    "linker_hand/g20_left/linkerhand_g20_left.urdf"
+)
+RIGHT_SOURCE_URDF = REPOSITORY / (
+    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+    "linker_hand/g20_right/linkerhand_g20_right.urdf"
+)
+
+
+def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
+    parent = np.asarray([0.0, 0.0, 1.0])
+    predicted = np.asarray([1.0, 0.0, 0.0])
+    cone_mismatch = np.asarray(
+        [math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
+    )
+    zero_mismatch = np.asarray(
+        [math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
+    )
+
+    assert _zero_sensitive_axis_error_rad(
+        predicted, cone_mismatch, parent
+    ) == pytest.approx(0.0, abs=1.0e-12)
+    assert math.degrees(
+        _zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
+    ) == pytest.approx(3.0, abs=1.0e-9)
+
+
+def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
+    parent = np.asarray([0.0, 0.0, 1.0])
+    cone = math.radians(32.0)
+    phase = math.radians(7.0)
+    predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
+    observed = Rotation.from_rotvec(parent * phase).apply(predicted)
+
+    error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
+
+    assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
+
+
+def _payload(transform: np.ndarray) -> dict[str, list[float]]:
+    return {
+        "translation_xyz_m": transform[:3, 3].tolist(),
+        "quaternion_xyzw": Rotation.from_matrix(
+            transform[:3, :3]
+        ).as_quat().tolist(),
+    }
+
+
+def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
+    axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
+    axis_parent /= np.linalg.norm(axis_parent)
+    centre_parent = np.asarray([0.012, -0.008, 0.021])
+    radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
+    radial = 0.035 * radial / np.linalg.norm(radial)
+    child_tag_mount = Rotation.from_euler(
+        "xyz", [1.1, -0.7, 0.45]
+    )
+    common_from_parent = np.eye(4)
+    common_from_parent[:3, :3] = Rotation.from_euler(
+        "xyz", [-0.8, 0.55, 1.3]
+    ).as_matrix()
+    common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
+    expected_axis = common_from_parent[:3, :3] @ axis_parent
+    expected_point = (
+        common_from_parent[:3, :3] @ centre_parent
+        + common_from_parent[:3, 3]
+    )
+
+    commands = list(range(0, 256, 16)) + [255]
+    records = []
+    for cycle in range(3):
+        for direction in ("decreasing", "increasing"):
+            for command in commands:
+                angle = math.radians(62.0) * (255.0 - command) / 255.0
+                motion = Rotation.from_rotvec(axis_parent * angle)
+                relative_rotation = motion * child_tag_mount
+                relative_translation = centre_parent + motion.apply(radial)
+                child_common = common_from_parent.copy()
+                child_common[:3, :3] = (
+                    common_from_parent[:3, :3]
+                    @ relative_rotation.as_matrix()
+                )
+                child_common[:3, 3] = (
+                    common_from_parent[:3, :3] @ relative_translation
+                    + common_from_parent[:3, 3]
+                )
+                state = [255.0] * 20
+                state[5] = float(command)
+                records.append(
+                    {
+                        "cycle": cycle,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_translation_xyz_m": relative_translation.tolist(),
+                        "relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
+                        "parent_pose_common": _payload(common_from_parent),
+                        "child_pose_common": _payload(child_common),
+                        "state_u8": state,
+                    }
+                )
+    return records, expected_axis, expected_point
+
+
+def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    curve = fit_rotation_joint_curve(records, zero_command_u8=255)
+    measurement = fit_joint_axis_measurement(
+        "thumb_cmc_roll", records, cycle=0, zero_command_u8=255
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+    assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
+    assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
+
+
+@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
+def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
+    joint: str,
+) -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    axis_parent = parent_rotation.inv().apply(expected_axis)
+    tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
+    tangent /= np.linalg.norm(tangent)
+    # Reproduce monocular planar-PnP depth bias: the centre trajectory remains
+    # precise in its dominant directions but receives a command-correlated
+    # component that makes a free 3-D plane normal substantially wrong.
+    biased_records = []
+    for record in records:
+        biased = dict(record)
+        point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
+        depth_bias = 0.30 * float(point @ tangent)
+        biased["relative_translation_xyz_m"] = (
+            point + depth_bias * axis_parent
+        ).tolist()
+        biased_records.append(biased)
+
+    measurement = fit_joint_axis_measurement(
+        joint, biased_records, cycle=0, zero_command_u8=255
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 0.003
+    assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
+    assert measurement.plane_rms_m < 0.003
+    assert measurement.radial_rms_m < 0.003
+
+
+def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    axis_parent = common_from_parent.inv().apply(expected_axis)
+    # Exact end-on depth is a gauge along the physical axis and therefore
+    # cannot alter the observable axis line.  An oblique camera has a small
+    # irreducible coupling between monocular depth and radial position; that
+    # case must be bounded by the residual/holdout gates, not asserted to be
+    # exactly recoverable from one view.
+    view_normal_parent = axis_parent
+    view_normal_common = common_from_parent.apply(view_normal_parent)
+    biased = []
+    for record in records:
+        changed = dict(record)
+        fraction = (255.0 - float(record["command_u8"])) / 255.0
+        depth_bias = 0.03 * (fraction - 0.5)
+        changed["relative_translation_xyz_m"] = (
+            np.asarray(record["relative_translation_xyz_m"], dtype=float)
+            + depth_bias * view_normal_parent
+        ).tolist()
+        biased.append(changed)
+
+    measurement = fit_joint_axis_measurement(
+        "thumb_cmc_pitch",
+        biased,
+        cycle=0,
+        zero_command_u8=255,
+        view_normal_common_xyz=view_normal_common,
+    )
+
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert measurement.axis_point_source == "pose_trajectory_image_plane"
+    assert measurement.pose_axis_line_rms_m < 1.0e-6
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+
+
+def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
+    records, expected_axis, _ = _arbitrary_tag_records()
+    assert not any(record["command_u8"] == 127 for record in records)
+
+    curve = fit_rotation_joint_curve(records, zero_command_u8=127)
+    measurement = fit_joint_axis_measurement(
+        "index_mcp_roll", records, cycle=0, zero_command_u8=127
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
+
+
+def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    physical_axis_parent = common_from_parent.inv().apply(expected_axis)
+    wrong_axis_parent = np.cross(
+        physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
+    )
+    wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
+    mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
+    contradictory = []
+    for record in records:
+        changed = dict(record)
+        angle = math.radians(62.0) * (
+            255.0 - float(record["command_u8"])
+        ) / 255.0
+        changed["relative_quaternion_xyzw"] = (
+            Rotation.from_rotvec(wrong_axis_parent * angle) * mount
+        ).as_quat().tolist()
+        contradictory.append(changed)
+
+    measurement = fit_joint_axis_measurement(
+        "index_dip",
+        contradictory,
+        cycle=0,
+        zero_command_u8=255,
+        axis_common_constraint=expected_axis,
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+
+
+def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
+    cameras = {
+        view: {
+            "serial_number": f"SERIAL_{view}",
+            "width": 1624,
+            "height": 1240,
+            "intrinsics_sha256": camera_info_fingerprint(
+                width=1624,
+                height=1240,
+                camera_matrix=np.asarray(
+                    [[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
+                ),
+            ),
+        }
+        for view in ("front", "side", "top")
+    }
+    transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
+    transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
+    transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
+    transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
+    transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
+    destination = tmp_path / "extrinsics.yaml"
+
+    dump_three_camera_extrinsics(
+        destination,
+        cameras=cameras,
+        front_from_view=transforms,
+        quality={
+            "passed": True,
+            "reprojection_rms_px": 0.3,
+            "maximum_rotation_repeatability_deg": 0.2,
+            "maximum_translation_repeatability_m": 0.001,
+            "front_side_captures": 15,
+            "front_top_captures": 15,
+        },
+    )
+    loaded = load_three_camera_extrinsics(destination)
+
+    assert loaded.cameras["front"].serial_number == "SERIAL_front"
+    assert np.allclose(loaded.transform("side"), transforms["side"])
+    assert np.allclose(loaded.transform("top"), transforms["top"])
+    assert loaded.camera_matches(
+        "front",
+        serial_number="SERIAL_front",
+        width=1624,
+        height=1240,
+        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
+    )
+    assert not loaded.camera_matches(
+        "front",
+        serial_number="WRONG_SERIAL",
+        width=1624,
+        height=1240,
+        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
+    )
+
+
+def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
+    joint = next(
+        element
+        for element in ET.parse(path).getroot().findall("joint")
+        if element.get("name") == name
+    )
+    origin = joint.find("origin")
+    axis = joint.find("axis")
+    xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
+    rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
+    axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
+    return np.block(
+        [
+            [Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
+            [np.asarray([[0.0, 0.0, 0.0, 1.0]])],
+        ]
+    ), axis_xyz / np.linalg.norm(axis_xyz)
+
+
+def _joint_limit(path: Path, name: str) -> tuple[float, float]:
+    joint = next(
+        element
+        for element in ET.parse(path).getroot().findall("joint")
+        if element.get("name") == name
+    )
+    limit = joint.find("limit")
+    return float(limit.get("lower")), float(limit.get("upper"))
+
+
+def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
+    offset = math.radians(7.3)
+    destination = write_zero_corrected_urdf(
+        source_urdf=SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="G20_LEFT_001",
+        offsets_rad={"thumb_cmc_yaw": offset},
+        timestamp="20260806_120000",
+    )
+    original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
+    corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
+    expected = original.copy()
+    expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
+        axis * offset
+    ).as_matrix()
+
+    assert destination != SOURCE_URDF
+    assert np.allclose(corrected, expected, atol=1.0e-12)
+    with pytest.raises(ValueError, match="refusing to overwrite"):
+        write_zero_corrected_urdf(
+            source_urdf=SOURCE_URDF,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": offset},
+            timestamp="20260806_120000",
+        )
+    with pytest.raises(ValueError, match="original CAD URDF"):
+        write_zero_corrected_urdf(
+            source_urdf=destination,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": offset},
+            timestamp="20260806_120001",
+        )
+    with pytest.raises(ValueError, match="finite and within"):
+        write_zero_corrected_urdf(
+            source_urdf=SOURCE_URDF,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": math.nan},
+            timestamp="20260806_120002",
+        )
+
+
+def test_urdf_writer_changes_only_the_16_active_zero_origins(
+    tmp_path: Path,
+) -> None:
+    before = SOURCE_URDF.read_bytes()
+    offsets = {
+        name: math.radians(0.25 * (index + 1))
+        for index, name in enumerate(ACTIVE_JOINTS)
+    }
+    destination = write_zero_corrected_urdf(
+        source_urdf=SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="G20_LEFT_001",
+        offsets_rad=offsets,
+        timestamp="20260807_180000",
+    )
+
+    assert len(offsets) == 16
+    assert SOURCE_URDF.read_bytes() == before
+    for name in ACTIVE_JOINTS:
+        original, axis = _joint_origin(SOURCE_URDF, name)
+        corrected, corrected_axis = _joint_origin(destination, name)
+        expected = original.copy()
+        expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
+            axis * offsets[name]
+        ).as_matrix()
+        assert np.allclose(corrected, expected, atol=1.0e-12)
+        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
+    for name in PASSIVE_JOINTS:
+        original, axis = _joint_origin(SOURCE_URDF, name)
+        corrected, corrected_axis = _joint_origin(destination, name)
+        assert np.allclose(corrected, original, atol=1.0e-12)
+        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
+    # A zero calibration must not silently expand mechanical/CAD safety
+    # limits.  Dynamic measured ranges remain in the calibration JSON.
+    for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
+        assert _joint_limit(destination, name) == pytest.approx(
+            _joint_limit(SOURCE_URDF, name)
+        )
+
+
+def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
+    values = np.asarray(
+        [travel * (255.0 - command) / 255.0 for command in range(256)]
+    )
+    values -= values[zero_command]
+    data = tuple(float(value) for value in values)
+    return JointCurveFit(
+        angle_rad=data,
+        decreasing_rad=data,
+        increasing_rad=data,
+        circle={},
+        maximum_monotonic_correction_rad=0.0,
+        maximum_hysteresis_rad=0.0,
+        quality={},
+    )
+
+
+def _solve_synthetic_offsets(
+    side: str,
+    offset_degrees: list[float],
+    *,
+    inject_oblique_optical_depth_bias: bool = False,
+    inject_secondary_root_axis_bias_degrees: float = 0.0,
+    inject_secondary_root_point_bias_m: float = 0.0,
+    inject_observer_cone_bias_degrees: float = 0.0,
+    pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
+    joint_maximum_offset_degrees: dict[str, float] | None = None,
+):
+    hand = get_hand_calibration_profile(side)
+    zero = get_zero_calibration_profile(side)
+    source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[hand.joint_specs[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in hand.measured_joints
+    }
+    if inject_secondary_root_axis_bias_degrees:
+        # Make the thumb root the higher-travel, directly observed direction,
+        # matching the real right-hand data where the short pinky splay arc is
+        # the less reliable root-axis orientation estimate.
+        curves["thumb_cmc_roll"] = _synthetic_curve(
+            int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
+            math.radians(70.0),
+        )
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in hand.joint_specs.items()
+    }
+    offsets = {
+        name: math.radians(value)
+        for name, value in zip(zero.direct_zero_joints, offset_degrees)
+    }
+    model = UrdfKinematicModel(source)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements: list[JointAxisMeasurement] = []
+    for cycle in range(3):
+        for joint in zero.axis_joints:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            axis, point = model.axis_line(
+                joint, zero_offsets=offsets, joint_angles=angles
+            )
+            point_common = base_rotation.apply(point) + base_translation
+            if (
+                inject_secondary_root_point_bias_m
+                and joint == f"{zero.reference_finger}_mcp_roll"
+            ):
+                # A repeatable monocular depth error on the second parallel
+                # root line must affect translation only, never palm rotation
+                # or the inferred thumb-roll zero.
+                point_common = point_common + base_rotation.apply(
+                    np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
+                )
+            view_normal_common = None
+            if (
+                inject_oblique_optical_depth_bias
+                and joint in zero.phase_parent_joint
+            ):
+                parent_axis = model.axis_line(
+                    zero.phase_parent_joint[joint],
+                    zero_offsets=offsets,
+                    joint_angles=angles,
+                )[0]
+                helper = (
+                    np.asarray([1.0, 0.0, 0.0])
+                    if abs(float(parent_axis[0])) < 0.8
+                    else np.asarray([0.0, 1.0, 0.0])
+                )
+                tilt_axis = np.cross(parent_axis, helper)
+                tilt_axis /= np.linalg.norm(tilt_axis)
+                view_normal = Rotation.from_rotvec(
+                    math.radians(15.0) * tilt_axis
+                ).apply(parent_axis)
+                view_normal_common = tuple(base_rotation.apply(view_normal))
+                # Simulate an independent planar-PnP depth error on the child
+                # Tag. It is large enough to drive the old 3-D phase solve to
+                # a configured offset bound.
+                point_common = point_common + 0.03 * np.asarray(
+                    view_normal_common
+                )
+            axis_common = base_rotation.apply(axis)
+            if (
+                inject_observer_cone_bias_degrees
+                and joint == "thumb_cmc_pitch"
+            ):
+                parent_axis = model.axis_line(
+                    zero.axis_parent_joint[joint],
+                    zero_offsets=offsets,
+                    joint_angles=angles,
+                )[0]
+                cone_normal = np.cross(axis, parent_axis)
+                cone_normal /= np.linalg.norm(cone_normal)
+                axis_common = base_rotation.apply(
+                    Rotation.from_rotvec(
+                        math.radians(inject_observer_cone_bias_degrees)
+                        * cone_normal
+                    ).apply(axis)
+                )
+            if (
+                inject_secondary_root_axis_bias_degrees
+                and joint == f"{zero.reference_finger}_mcp_roll"
+            ):
+                helper = np.asarray([0.0, 0.0, 1.0])
+                if abs(float(axis_common @ helper)) > 0.8:
+                    helper = np.asarray([0.0, 1.0, 0.0])
+                bias_axis = np.cross(axis_common, helper)
+                bias_axis /= np.linalg.norm(bias_axis)
+                axis_common = Rotation.from_rotvec(
+                    math.radians(inject_secondary_root_axis_bias_degrees)
+                    * bias_axis
+                ).apply(axis_common)
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(axis_common),
+                    point_common_xyz_m=tuple(point_common),
+                    condition_state_u8=tuple(state),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                    view_normal_common_xyz=view_normal_common,
+                    pose_axis_line_rms_m=(
+                        pose_axis_line_rms_by_joint_m or {}
+                    ).get(joint, 0.0),
+                )
+            )
+    result = solve_urdf_zero_offsets(
+        source_urdf=source,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type=side,
+        joint_maximum_offset_rad={
+            name: math.radians(value)
+            for name, value in (joint_maximum_offset_degrees or {}).items()
+        },
+    )
+    return zero, result
+
+
+def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
+    zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, value in result.direct_offsets_rad.items():
+        assert value == pytest.approx(static_policy.get(name, 0.0))
+
+
+def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
+    right = get_zero_calibration_profile("right")
+    left = get_zero_calibration_profile("left")
+
+    assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
+    assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
+    assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
+    assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
+
+
+def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
+    )
+    reference_roll = f"{zero.reference_finger}_mcp_roll"
+    assert result.passed is True
+    assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
+        0.0, abs=1.0e-12
+    )
+
+
+def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_secondary_root_axis_bias_degrees=15.0,
+    )
+
+    assert result.passed is True
+    expected = dict(
+        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
+    )
+    expected.update(
+        {
+            name: math.degrees(value)
+            for name, value in {
+                **zero.fixed_direct_zero_offsets_rad,
+                **zero.static_output_zero_offsets_rad,
+            }.items()
+        }
+    )
+    for name, value in expected.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            value, abs=0.05
+        )
+
+
+def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_secondary_root_point_bias_m=0.02,
+    )
+
+    assert result.passed is True
+    assert math.degrees(
+        result.direct_offsets_rad["thumb_cmc_roll"]
+    ) == pytest.approx(2.0, abs=0.05)
+    assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
+
+
+def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
+    offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
+    _, result = _solve_synthetic_offsets(
+        "right",
+        offsets,
+        joint_maximum_offset_degrees={"thumb_mcp": 45.0},
+    )
+
+    assert result.passed is True
+    assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
+    assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
+    assert "thumb_ip" not in result.validation_error_by_joint_rad
+    for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
+        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
+    for name in ("pinky_mcp_pitch", "pinky_pip"):
+        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
+    assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
+
+
+def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_oblique_optical_depth_bias=True,
+    )
+
+    assert result.passed is True
+    expected = dict(
+        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
+    )
+    expected.update(
+        {
+            name: math.degrees(value)
+            for name, value in {
+                **zero.fixed_direct_zero_offsets_rad,
+                **zero.static_output_zero_offsets_rad,
+            }.items()
+        }
+    )
+    for name, value in expected.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            value, abs=0.05
+        )
+
+
+def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
+    _, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_observer_cone_bias_degrees=8.0,
+    )
+
+    assert result.passed is False
+    assert result.failure_reasons["thumb_cmc_yaw"] == (
+        "zero_axis_cone_mismatch_too_large"
+    )
+
+
+def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
+    _, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
+    )
+
+    assert result.passed is False
+    assert result.failure_reasons["thumb_cmc_pitch"] == (
+        "zero_phase_axis_line_residual_too_large"
+    )
+
+
+def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
+    zero = get_zero_calibration_profile("left")
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[JOINT_SPECS[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in MEASURED_JOINTS
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in JOINT_SPECS.items()
+    }
+    true_offsets = {
+        name: math.radians(value)
+        for name, value in zip(
+            DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
+        )
+    }
+    model = UrdfKinematicModel(SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements = []
+    yaw_axis_without_clearance = None
+    yaw_axis_with_clearance = None
+    for cycle in range(3):
+        for joint in AXIS_JOINTS:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            angles = _angles_from_state(
+                state, curves=curves, motor_by_joint=motor_by_joint
+            )
+            axis, point = model.axis_line(
+                joint,
+                zero_offsets=true_offsets,
+                joint_angles=angles,
+            )
+            if joint == "thumb_cmc_yaw":
+                yaw_axis_with_clearance = axis.copy()
+                baseline_angles = _angles_from_state(
+                    baseline, curves=curves, motor_by_joint=motor_by_joint
+                )
+                yaw_axis_without_clearance = model.axis_line(
+                    joint,
+                    zero_offsets=true_offsets,
+                    joint_angles=baseline_angles,
+                )[0]
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=tuple(state),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+    )
+
+    assert math.degrees(
+        math.acos(
+            np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
+        )
+    ) > 1.0
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, expected in true_offsets.items():
+        if name in static_policy:
+            expected = static_policy[name]
+        assert result.direct_offsets_rad[name] == pytest.approx(
+            expected, abs=math.radians(0.05)
+        )
+    assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
+    assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
+        0.0, abs=math.radians(0.05)
+    )
+    for target in INHERITED_ZERO_JOINTS:
+        assert result.all_active_offsets_rad[target] == pytest.approx(
+            0.0, abs=1.0e-12
+        )
+    assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
+
+
+def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
+    hand = get_hand_calibration_profile("right")
+    zero = get_zero_calibration_profile("right")
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[hand.joint_specs[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in hand.measured_joints
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in hand.joint_specs.items()
+    }
+    true_offsets = {
+        name: math.radians(value)
+        for name, value in zip(
+            zero.direct_zero_joints,
+            [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        )
+    }
+    model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements: list[JointAxisMeasurement] = []
+    for cycle in range(3):
+        states: dict[str, list[float]] = {}
+        lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
+        for joint in zero.axis_joints:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            states[joint] = state
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            lines[joint] = model.axis_line(
+                joint,
+                zero_offsets=true_offsets,
+                joint_angles=angles,
+            )
+        original_lines = {
+            name: (axis.copy(), point.copy())
+            for name, (axis, point) in lines.items()
+        }
+        for observer, parent in zero.phase_parent_joint.items():
+            parent_axis, parent_point = lines[parent]
+            original_parent_axis, original_parent_point = original_lines[parent]
+            child_axis, original_child_point = original_lines[observer]
+            radial = original_child_point - original_parent_point
+            radial -= original_parent_axis * float(
+                radial @ original_parent_axis
+            )
+            # Preserve angular phase while deliberately corrupting link radius
+            # and along-axis depth. These components must not move a zero.
+            lines[observer] = (
+                child_axis,
+                parent_point + 1.25 * radial + 0.02 * parent_axis,
+            )
+        for joint in zero.axis_joints:
+            axis, point = lines[joint]
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=tuple(states[joint]),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=RIGHT_SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type="right",
+    )
+
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, expected in true_offsets.items():
+        if name in static_policy:
+            expected = static_policy[name]
+        assert result.direct_offsets_rad[name] == pytest.approx(
+            expected, abs=math.radians(0.05)
+        )
+    for target in zero.inherited_zero_joints:
+        assert result.all_active_offsets_rad[target] == pytest.approx(
+            0.0, abs=1.0e-12
+        )
+    assert set(result.all_active_offsets_rad) == set(hand.active_joints)

src/gui_control/gui_control/config/constants.py

@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
             "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
-            "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
-            "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
-            "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
-            "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+            "OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
+            "拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
+            "拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
+            "拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
             "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
             "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],

src/linker_hand_ros2_sdk/launch/test.bak

@@ -1,31 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_left',
-            output='screen',
-            parameters=[{
-                'hand_type': 'left',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_right',
-            output='screen',
-            parameters=[{
-                'hand_type': 'right',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-    ])

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak

@@ -1,444 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-from pymodbus.client import ModbusSerialClient
-from typing import List, Dict
-import numpy as np
-
-_INTERVAL = 0.006  # 8 ms
-
-class LinkerHandL6RS485:
-    """L6机械手 Modbus-RTU 控制类"""
-    
-    # 6个关节名称
-    JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", 
-                   "middle_pitch", "ring_pitch", "little_pitch"]
-    
-    # 手指名称
-    FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
-    
-    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
-        """
-        初始化L6机械手
-        hand_id: 右手0x27(39), 左手0x28(40)
-        modbus_port: 串口设备路径
-        baudrate: 波特率，固定115200
-        """
-        self.slave = hand_id
-        self.cli = ModbusSerialClient(
-            port=modbus_port, 
-            baudrate=baudrate,
-            bytesize=8, 
-            parity="N", 
-            stopbits=1,
-            timeout=0.05
-        )
-        # pymodbus 3.5.1 需要显式连接
-        self.connected = self.cli.connect()
-        if not self.connected:
-            raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
-
-    def _read_input_registers(self, address: int, count: int) -> List[int]:
-        """读取输入寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
-        return result.registers
-
-    def _write_register(self, address: int, value: int):
-        """写入单个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_register(address=address, value=value, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
-
-    def _write_registers(self, address: int, values: List[int]):
-        """写入多个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_registers(address=address, values=values, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
-
-    # --------------------------------------------------
-    # 基础读取接口
-    # --------------------------------------------------
-    
-    def read_angles(self) -> List[int]:
-        """读取6个关节角度 (输入寄存器 0-5)"""
-        return self._read_input_registers(0, 6)
-
-    def read_torques(self) -> List[int]:
-        """读取6个关节转矩 (输入寄存器 6-11)"""
-        return self._read_input_registers(6, 6)
-
-    def read_speeds(self) -> List[int]:
-        """读取6个关节速度 (输入寄存器 12-17)"""
-        return self._read_input_registers(12, 6)
-
-    def read_temperatures(self) -> List[int]:
-        """读取6个关节温度 (输入寄存器 18-23)"""
-        return self._read_input_registers(18, 6)
-
-    def read_error_codes(self) -> List[int]:
-        """读取6个关节错误码 (输入寄存器 24-29)"""
-        return self._read_input_registers(24, 6)
-
-    # --------------------------------------------------
-    # 压力传感器接口
-    # --------------------------------------------------
-    
-    # def _pressure(self, finger: int) -> List[int]:
-    #     """内部：选手指 → 读压力数据"""
-    #     # 选择手指 (保持寄存器 36)
-    #     self._write_register(36, finger)
-    #     time.sleep(_INTERVAL)
-    #     # 读取压力数据 (输入寄存器 52-122)
-    #     return np.array(self._read_input_registers(52, 71))
-    def _pressure(self, finger: int) -> np.ndarray:
-        """
-        6x12 (72点) 矩阵尺寸。
-        Modbus 地址 60/62。
-        """
-        rows = 12  # 12 行
-        cols = 6   # 6 列
-        finger_size = rows * cols  # 72 个数据点
-        
-        # modbus 地址和计数
-        write_address = 60  # 写入手指选择
-        read_address = 62   # 读取压力数据
-        read_count = 96     # 读取 96 个寄存器
-        skip_count = 10     # 跳过前 10 个校验点
-        
-        # 0. 参数校验和手指写入值确定
-        if finger < 1 or finger > 5:
-            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
-            
-        finger_write_value = finger 
-        
-        # 1. 写入手指选择寄存器 (地址 60)
-        time.sleep(0.008)
-        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
-        if wrsp.isError():
-            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
-
-        # 写入后等待片刻
-        time.sleep(0.008) 
-        
-        # 2. 读取地址 62 的数据
-        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
-        
-        if rrsp.isError():
-            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
-            
-        registers_16bit: List[int] = rrsp.registers 
-        
-        # 3. 核心数据处理
-        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
-        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
-        
-        # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
-        effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
-        # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
-        start_idx = 0 
-        end_idx = finger_size  # 72
-        
-        finger_data_flat = effective_data[start_idx:end_idx]
-        
-        # d. 验证数据长度
-        if finger_data_flat.size != finger_size:
-            raise ValueError(
-                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
-                f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
-            )
-            
-        # e. 重塑为二维矩阵 (12 行 6 列)
-        finger_matrix = finger_data_flat.reshape((rows, cols))
-                
-        return finger_matrix
-
-    def read_pressure_thumb(self) -> np.ndarray:
-        """读取大拇指压力数据"""
-        return np.array(self._pressure(1), dtype=np.uint8)
-
-    def read_pressure_index(self) -> np.ndarray:
-        """读取食指压力数据"""
-        return np.array(self._pressure(2), dtype=np.uint8)
-
-    def read_pressure_middle(self) -> np.ndarray:
-        """读取中指压力数据"""
-        return np.array(self._pressure(3), dtype=np.uint8)
-
-    def read_pressure_ring(self) -> np.ndarray:
-        """读取无名指压力数据"""
-        return np.array(self._pressure(4), dtype=np.uint8)
-
-    def read_pressure_little(self) -> np.ndarray:
-        """读取小拇指压力数据"""
-        return np.array(self._pressure(5), dtype=np.uint8)
-
-    # --------------------------------------------------
-    # 版本信息接口
-    # --------------------------------------------------
-    
-    def read_versions(self) -> Dict[str, int]:
-        """读取版本信息 (输入寄存器 148-155)"""
-        result = self._read_input_registers(148, 8)
-        
-        return {
-            "hand_freedom": result[0],
-            "hand_version": result[1],
-            "hand_number": result[2],
-            "hand_direction": result[3],
-            "software_version_major": result[4],
-            "software_version_minor": result[5] if len(result) > 5 else 0,
-            "software_version_revision": result[6] if len(result) > 6 else 0,
-            "hardware_version": result[7] if len(result) > 7 else 0
-        }
-
-    # --------------------------------------------------
-    # 写入接口
-    # --------------------------------------------------
-    
-    def write_angles(self, vals: List[int]):
-        """设置6个关节角度 (保持寄存器 0-5)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(0, vals)
-
-    def write_torques(self, vals: List[int]):
-        """设置6个关节转矩 (保持寄存器 6-11)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(6, vals)
-
-    def write_speeds(self, vals: List[int]):
-        """设置6个关节速度 (保持寄存器 12-17)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(12, vals)
-
-    # --------------------------------------------------
-    # 上下文管理
-    # --------------------------------------------------
-    
-    def close(self):
-        """关闭连接"""
-        if self.connected:
-            self.cli.close()
-            self.connected = False
-    
-    def __enter__(self):
-        return self
-    
-    def __exit__(self, exc_type, exc_val, exc_tb):
-        self.close()
-
-    # --------------------------------------------------
-    # API固定接口函数
-    # --------------------------------------------------
-    
-    def is_valid_6xuint8(self, lst) -> bool:
-        """验证6个0-255的整数列表"""
-        if len(lst) != 6:
-            return False
-        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
-    
-    def set_joint_positions(self, joint_angles=None):
-        """设置关节位置"""
-        joint_angles = joint_angles or [0] * 6
-        self.write_angles(joint_angles)
-
-    def set_speed(self, speed=None):
-        """设置速度"""
-        speed = speed or [200] * 6
-        self.write_speeds(speed)
-    
-    def set_torque(self, torque=None):
-        """设置扭矩"""
-        torque = torque or [200] * 6
-        self.write_torques(torque)
-
-    def set_current(self, current=None):
-        """设置电流 (L6不支持)"""
-        print("当前L6不支持设置电流", flush=True)
-
-    def get_version(self) -> list:
-        """获取版本信息"""
-        versions = self.read_versions()
-        return [
-            versions.get("hand_freedom", 0),
-            versions.get("hand_version", 0),
-            versions.get("hand_number", 0),
-            versions.get("hand_direction", 0),
-            versions.get("software_version_major", 0),
-            versions.get("hardware_version", 0)
-        ]
-
-    def get_current(self):
-        """获取电流 (L6不支持)"""
-        print("当前L6不支持获取电流", flush=True)
-        return []
-
-    def get_state(self) -> list:
-        """获取关节状态"""
-        return self.read_angles()
-    
-    def get_state_for_pub(self) -> list:
-        return self.get_state()
-
-    def get_current_status(self) -> list:
-        return self.get_state()
-    
-    def get_speed(self) -> list:
-        """获取当前速度"""
-        return self.read_speeds()
-    
-    def get_joint_speed(self) -> list:
-        return self.get_speed()
-    
-    def get_touch_type(self) -> int:
-        """获取压感类型 (2=矩阵式)"""
-        return 2
-    
-    def get_normal_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_tangential_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_approach_inc(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_touch(self) -> list:
-        return [-1] * 5
-
-    def get_thumb_matrix_touch(self,sleep_time=0):
-        return self._pressure(1)
-
-    def get_index_matrix_touch(self,sleep_time=0):
-        return self._pressure(2)
-
-    def get_middle_matrix_touch(self,sleep_time=0):
-        return self._pressure(3)
-
-    def get_ring_matrix_touch(self,sleep_time=0):
-        return self._pressure(4)
-
-    def get_little_matrix_touch(self,sleep_time=0):
-        return self._pressure(5)
-        
-    def get_matrix_touch(self) -> list:
-        """获取压感数据：矩阵式"""
-        return [self._pressure(1), self._pressure(2), self._pressure(3), 
-                self._pressure(4), self._pressure(5)]
-    
-    def get_matrix_touch_v2(self) -> list:
-        """获取压感数据：矩阵式"""
-        return self.get_matrix_touch()
-    
-    def get_torque(self) -> list:
-        """获取当前扭矩"""
-        return self.read_torques()
-    
-    def get_temperature(self) -> list:
-        """获取当前电机温度"""
-        return self.read_temperatures()
-    
-    def get_fault(self) -> list:
-        """获取当前电机故障码"""
-        return self.read_error_codes()
-    
-    def get_serial_number(self):
-        return [0] * 6
-
-    # --------------------------------------------------
-    # 便捷方法
-    # --------------------------------------------------
-    
-    def relax(self):
-        """所有手指伸直"""
-        self.set_joint_positions([255] * 6)
-    
-    def fist(self):
-        """所有手指握拳"""
-        self.set_joint_positions([0] * 6)
-    
-    def dump_status(self):
-        """打印状态信息"""
-        print("=" * 50)
-        print("L6机械手状态信息")
-        print("=" * 50)
-        
-        try:
-            # 关节状态
-            angles = self.read_angles()
-            torques = self.read_torques()
-            speeds = self.read_speeds()
-            temps = self.read_temperatures()
-            errors = self.read_error_codes()
-            
-            print("关节状态:")
-            for i, name in enumerate(self.JOINT_NAMES):
-                print(f"  {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
-                      f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
-            
-            # 版本信息
-            versions = self.read_versions()
-            print("\n版本信息:")
-            for key, value in versions.items():
-                print(f"  {key:20s}: {value}")
-            
-            # 压力传感器测试
-            print("\n压力传感器测试:")
-            thumb_pressure = self.read_pressure_thumb()
-            print(f"大拇指压力数据长度: {len(thumb_pressure)}")
-            
-        except Exception as e:
-            print(f"读取状态时出错: {e}")
-        
-        print("=" * 50)
-
-
-# ------------------- 演示程序 -------------------
-if __name__ == "__main__":
-    # 使用示例
-    try:
-        with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
-            print("连接成功!")
-            
-            # 打印状态信息
-            hand.dump_status()
-            
-            # 测试基本控制
-            print("\n测试控制功能...")
-            print("伸直手指...")
-            hand.relax()
-            time.sleep(2)
-            
-            print("握拳...")
-            hand.fist()
-            time.sleep(2)
-            
-            print("恢复伸直...")
-            hand.relax()
-            
-            # 测试压力传感器
-            print("\n测试压力传感器...")
-            thumb_matrix = hand.get_thumb_matrix_touch()
-            print(f"大拇指压力数据: {len(thumb_matrix)}个点")
-            
-            # 获取所有手指压力数据
-            all_matrices = hand.get_matrix_touch()
-            for i, name in enumerate(hand.FINGER_NAMES):
-                matrix = all_matrices[i]
-                print(f"{name}手指压力数据长度: {len(matrix)}")
-            
-    except Exception as e:
-        print(f"错误: {e}")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak

@@ -1,414 +0,0 @@
-#!/usr/bin/env python3 
-# -*- coding: utf-8 -*-
-'''
-编译: colcon build --symlink-install
-启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
-'''
-from re import A
-import rclpy,sys                                     # ROS2 Python接口库
-import time
-import numpy as np
-from rclpy.node import Node                      # ROS2 节点类
-from rclpy.clock import Clock
-from std_msgs.msg import String, Header, Float32MultiArray
-from sensor_msgs.msg import JointState, PointCloud2, PointField
-import time, json, threading
-from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
-from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
-from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
-
-
-class LinkerHand(Node):
-    def __init__(self, name):
-        super().__init__(name)
-        # 声明参数（带默认值）
-        self.declare_parameter('hand_type', 'left')
-        self.declare_parameter('hand_joint', 'L6')
-        self.declare_parameter('is_touch', False)
-        self.declare_parameter('can', 'can0')
-        self.declare_parameter('modbus', "None")        
-
-        # ros时间获取
-        self.stamp_clock = Clock()
-        # 获取参数值
-        self.hand_type = self.get_parameter('hand_type').value
-        self.hand_joint = self.get_parameter('hand_joint').value
-        self.is_touch = self.get_parameter('is_touch').value
-        self.can = self.get_parameter('can').value
-        self.modbus = self.get_parameter('modbus').value
-        self.sdk_v = 2
-        self.sleep_time = 0.005
-        self.cmd_lock = False
-        self.last_hand_post_cmd = None # 最新手指位置命令
-        self.last_hand_vel_cmd = None # 最新手指速度命令
-        self.last_hand_eff_cmd = None # 最新手指力矩命令
-
-        self.last_hand_state = [-1] * 10
-        self.last_hand_vel = [-1] * 10
-        self.force = [[-1] * 5] * 4
-        self.matrix_dic = {
-            "stamp":{
-                "sec": 0,
-                "nanosec": 0,
-            },
-            "thumb_matrix":[[-1] * 6 for _ in range(12)],
-            "index_matrix":[[-1] * 6 for _ in range(12)],
-            "middle_matrix":[[-1] * 6 for _ in range(12)],
-            "ring_matrix":[[-1] * 6 for _ in range(12)],
-            "little_matrix":[[-1] * 6 for _ in range(12)]
-        }
-        # 压感矩阵合值，单位g 克
-        self.matrix_mass_dic = {
-            "stamp":{
-                "secs": 0,
-                "nsecs": 0,
-            },
-            "thumb_mass":[-1],
-            "index_mass":[-1],
-            "middle_mass":[-1],
-            "ring_mass":[-1],
-            "little_mass":[-1]
-        }
-        self.last_hand_info = {
-            "version": [-1], # Dexterous hand version number
-            "hand_joint": self.hand_joint, # Dexterous hand joint type
-            "speed": [-1] * 10, # Current speed threshold of the dexterous hand
-            "current": [-1] * 10, # Current of the dexterous hand
-            "fault": [-1] * 10, # Current fault of the dexterous hand
-            "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
-            "torque": [-1] * 10, # Current torque of the dexterous hand
-            "is_touch":self.is_touch,
-            "touch_type": -1,
-            "finger_order": None # Finger motor order
-        }
-        self.version = []
-        self.touch_type = -1
-        self.hz = 1.0/60.0
-
-        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
-        self._init_hand()
-        time.sleep(1)
-        self.run_count = 0 # 计数器，用于记录运行次数
-        self.timer = self.create_timer(0.01, self.run)  # 100 Hz
-        self.thread_pub_state = threading.Thread(target=self.pub_state)
-        self.thread_pub_state.daemon = True
-        self.thread_pub_state.start()
-
-    def _init_hand(self):
-        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
-        time.sleep(0.1)
-        self.touch_type = self.api.get_touch_type()
-        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
-        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
-        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
-        if self.is_touch == True:
-            if self.touch_type > 1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
-                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
-                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
-                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
-            elif self.touch_type != -1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
-                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
-            else:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
-                self.is_touch = False
-        self.embedded_version = self.api.get_embedded_version()
-        pose = None
-        torque = [200, 200, 200, 200, 200]
-        speed = [200, 250, 250, 250, 250]
-        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
-            pose = [200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250]
-            # O6 最大速度阈值
-            speed = [200, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L7":
-            # The data length of L7 is 7, reinitialize here
-            pose = [255, 200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250, 250]
-            speed = [120, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L10":
-            torque = [255] * 10
-            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
-            speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L20":
-            pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
-        elif self.hand_joint == "L21":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        elif self.hand_joint == "L25":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        if pose is not None:
-            for i in range(1): 
-                self.api.set_speed(speed=speed)
-                time.sleep(0.1)
-                self.api.set_torque(torque=torque)
-                time.sleep(0.1)
-                self.api.finger_move(pose=pose)
-                time.sleep(0.1)
-
-    def list_check(self,pose):
-        if isinstance(pose, list) == False:
-            return False
-        if len(self.last_hand_post_cmd) != len(pose):
-            return False
-        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
-
-    def hand_control_cb(self, msg):
-        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
-            self.last_hand_post_cmd = msg.position
-        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
-            self.last_hand_vel_cmd = msg.velocity
-        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
-            self.last_hand_eff_cmd = msg.effort
-
-    def run(self):
-        if self.sdk_v == 1:
-            self.sleep_time = 0.009
-        if self.hand_state_pub.get_subscription_count() > 0:
-            # 优先获取手指状态并且发布
-            self.last_hand_state = self.api.get_state()
-            time.sleep(0.003)
-            self.last_hand_vel = self.api.get_joint_speed()
-            time.sleep(0.002)
-        if self.cmd_lock == False:
-            if self.last_hand_post_cmd != None:
-                self.api.finger_move(pose=self.last_hand_post_cmd)
-                self.last_hand_post_cmd = None
-            if self.last_hand_vel_cmd != None:
-                vel = list(self.last_hand_vel_cmd)
-                if all(x == 0 for x in vel):
-                    pass
-                else:
-                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L7" and len(vel) == 7:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L10" and len(vel) == 10:
-                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L20" and len(vel) == 20:
-                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L21" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L25" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                self.last_hand_vel_cmd = None
-            time.sleep(0.003)
-            if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                """单点式压力传感器"""
-                self.force = self.api.get_force()
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                """矩阵式压力传感器"""
-                if self.run_count == 3:
-                    self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 4:
-                    self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 5:
-                    self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 6:
-                    self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 7:
-                    self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
-                time.sleep(0.005)
-            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
-                """手部信息"""
-                self.last_hand_info = {
-                    "version": self.embedded_version, # Dexterous hand version number
-                    "hand_joint": self.hand_joint, # Dexterous hand joint type
-                    "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
-                    "current": self.api.get_current(), # Current of the dexterous hand
-                    "fault": self.api.get_fault(), # Current fault of the dexterous hand
-                    "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
-                    "torque": self.api.get_torque(), # Current torque of the dexterous hand
-                    "is_touch":self.is_touch,
-                    "touch_type": self.touch_type,
-                    "finger_order": self.api.get_finger_order() # Finger motor order
-                }
-            if self.run_count == 9:
-                self.run_count = 0
-            self.run_count += 1
-            time.sleep(0.003)
-
-
-    def pub_state(self):
-        while True:
-            if self.hand_state_pub.get_subscription_count() > 0:
-                msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
-                self.hand_state_pub.publish(msg)
-            if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                msg = Float32MultiArray()
-                msg.data = [float(val) for sublist in self.force for val in sublist]
-                self.touch_pub.publish(msg)
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                # 发布矩阵压感数据JSON格式
-                self.pub_matrix_dic()
-                # 发布矩阵压感和值JSON格式
-                self.pub_matrix_mass(dic=self.matrix_dic)
-                # 发布矩阵压感点云格式
-                self.pub_matrix_point_cloud()
-            if self.hand_info_pub.get_subscription_count() > 0:
-                msg = String()
-                msg.data = json.dumps(self.last_hand_info)
-                self.hand_info_pub.publish(msg)
-            time.sleep(self.hz)
-
-    def pub_matrix_mass(self, dic):
-        """发布矩阵数据合值 单位g 克 JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_mass_dic["stamp"]["secs"] = t_secs
-        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
-        self.matrix_mass_dic["unit"] = "g"
-        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
-        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
-        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
-        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
-        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
-        msg.data = json.dumps(self.matrix_mass_dic)
-        self.matrix_touch_mass_pub.publish(msg)
-
-    def pub_matrix_point_cloud(self):
-        """发布矩阵数据点云格式"""
-        tmp_dic = self.matrix_dic.copy()
-        del tmp_dic['stamp']               # 去掉时间戳字段
-        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数
-        # 摊平到一维：360 个 float
-        flat_list = [v for frame in all_matrices for v in frame]  # 360
-        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
-        fields = [PointField(
-            name='val',
-            offset=0,
-            datatype=PointField.UINT8,
-            count=1
-        )]
-        pc = PointCloud2()
-        pc.header.stamp = self.stamp_clock.now().to_msg()
-        pc.header.frame_id = ''
-        pc.height = 1
-        pc.width = flat.size         # 360
-        pc.fields = fields
-        pc.is_bigendian = False
-        pc.point_step = 1            # 1 个 float32
-        pc.row_step = pc.point_step * pc.width
-        pc.data = flat.tobytes()     # 1440 字节
-        self.matrix_touch_pub_pc.publish(pc)
-
-    def pub_matrix_dic(self):
-        """发布矩阵数据JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_dic["stamp"]["secs"] = t_secs
-        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
-        msg.data = json.dumps(self.matrix_dic)
-        self.matrix_touch_pub.publish(msg)
-
-    def joint_state_msg(self, pose,vel=[]):
-        joint_state = JointState()
-        joint_state.header = Header()
-        joint_state.header.stamp = self.get_clock().now().to_msg()
-        joint_state.name = self.api.get_finger_order()
-        joint_state.position = [float(x) for x in pose]
-        if len(vel) > 1:
-            joint_state.velocity = [float(x) for x in vel]
-        else:
-            joint_state.velocity = [0.0] * len(pose)
-        joint_state.effort = [0.0] * len(pose)
-        return joint_state
-    
-    
-
-    
-
-    def hand_setting_cb(self,msg):
-        '''控制命令回调'''
-        data = json.loads(msg.data)
-        print(f"Received setting command: {data['setting_cmd']}",flush=True)
-        try:
-            if data["params"]["hand_type"] == "left":
-                hand = self.api
-                hand_left = True
-            elif data["params"]["hand_type"] == "right":
-                hand = self.api
-                hand_right = True
-            else:
-                print("Please specify the hand part to be set",flush=True)
-                return
-            self.cmd_lock = True
-            # Set maximum torque
-            if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
-                torque = list(data["params"]["torque"])
-                hand.set_torque(torque=torque)
-                
-            if data["setting_cmd"] == "set_speed": # Set speed
-                if isinstance(data["params"]["speed"], list) == True:
-                    speed = data["params"]["speed"]
-                    hand.set_speed(speed=speed)
-                else:
-                    ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
-            if data["setting_cmd"] == "clear_faults": # Clear faults
-                if hand_left == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 left hand cannot clear faults")
-                elif hand_right == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 right hand cannot clear faults")
-                else:
-                    hand.clear_faults()
-            if data["setting_cmd"] == "get_faults": # Get faults
-                f = hand.get_fault()
-                ColorMsg(msg=f"Get faults: {f}")
-            if data["setting_cmd"] == "electric_current": # Get current
-                ColorMsg(msg=f"Get current: {hand.get_current()}")
-            if data["setting_cmd"] == "set_electric_current": # Set current
-                if isinstance(data["params"]["current"], list) == True:
-                    hand.set_current(data["params"]["current"])
-            if data["setting_cmd"] == "show_fun_table": # Get faults
-                f = hand.show_fun_table()
-        except:
-            print("命令参数错误")
-            self.cmd_lock = False
-        finally:
-            self.cmd_lock = False
-
-
-    def close_can(self):
-        self.api.open_can.close_can(can=self.can)
-        sys.exit(0)
-
-        
-def main(args=None):
-    try:
-        rclpy.init(args=args)
-        node = LinkerHand("linker_hand_sdk")
-        embedded_version = node.embedded_version
-        if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) == 6 and node.hand_joint == "L10":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        else:
-            ColorMsg(msg=f"SDK V1", color="green")
-            node.sdk_v = 1
-        rclpy.spin(node)         # 主循环，监听 ROS 回调
-    except KeyboardInterrupt:
-        print("收到 Ctrl+C，准备退出...")
-    finally:
-        # node.close_can()         # 关闭 CAN 或其他硬件资源
-        # node.destroy_node()      # 销毁 ROS 节点
-        # rclpy.shutdown()         # 关闭 ROS
-        print("程序已退出。")