G20四指单独标定（少末端tag）

.gitignore

@@ -62,7 +62,7 @@ Thumbs.db
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
 /calibration_output/
-/config/g20_three_camera_extrinsics.yaml
+/config/*_three_camera_extrinsics.yaml
 *.wear_check.json
 *.checkpoint.json
 *.verification.json
@@ -92,3 +92,5 @@ candump-*
 # Local Codex/agent workspace metadata
 /.agents/
 /.codex/
+/.codebuddy/
+/.zcode/

src/g20_thumb_apriltag_calibration/README.md

@@ -1,5 +1,136 @@
 # G20 左右手 AprilTag 标定
 
+## G20右手正式一键标定
+
+固定三相机和15张Tag安装完成后，用户只运行：
+
+```bash
+ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
+```
+
+开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经
+完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的
+独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与
+最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据
+当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
+失败、把会话拉回靠前的关节。需要强制从第一个关节
+重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
+一个完整同步任务断点；只有两边数据都完整时才复用。
+
+命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
+11个可求解主动关节URDF零位修正（`thumb_mcp`固定CAD零位）、
+17条实测命令曲线以及4条
+源URDF mimic派生DIP曲线发布。
+终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。
+
+正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
+JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
+全部通过后更新。
+
+## G20右手15-Tag底层调试入口
+
+以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简
+schema v4 JSON，不再生成schema v5运行文件。
+
+新布局用独立参数启用，原有左右手11-Tag流程仍默认使用
+`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
+  hand_type:=right \
+  tag_layout:=g20_right_15 \
+  serial_number:=G20_RIGHT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
+```
+
+15张Tag均为16 mm `tag36h11`。正面为
+`0,1,2,3,10,11,12,13`，侧面为
+`4,5,6,15,17`，上面为`8,9`；ID `7,14,16,18`不再粘贴。详细角色以
+`config/three_camera_tags_g20_right_15.yaml`为唯一软件配置源。程序执行4项拇指任务，
+以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
+侧面PIP，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过
+各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic
+multiplier生成，不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检
+0/127/255可见性；
+
+基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
+固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
+允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不
+放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的
+实时Tag。锁定后不得移动相机、手掌底座或整只手，否则缓存参考失效，必须重新启动
+标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
+任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把
+采集质量良好的任务误判为可见性失败。
+
+取消末端Tag后，四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准；
+因此PIP动态曲线仍为视觉实测，但PIP的URDF静态零位保留源CAD，不猜测写入。
+正式修正范围为拇指4个主动关节，以及四指各自的MCP roll/MCP pitch，共12个。
+侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出；
+同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时
+运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
+等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
+`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。
+同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
+不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且
+当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
+退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
+"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
+不超过8，会把非roll任务四轮正式速度最多提高到预检速度的1.5倍；否则保持原保守速度。
+四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
+零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
+四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
+预检、正式四轮和拟合重扫始终使用速度5。
+正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈；
+方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
+替代静态保持姿态。
+前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
+每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化，但同一任务第1轮
+已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
+IPPE镜像解。baseline标准接近和全部质量门限保持不变。
+电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
+排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放
+`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
+
+当前15-Tag产品流程发布精简schema v4：21条运行时曲线中，17条来自当前会话的
+视觉实测，四指DIP按源URDF受保护的mimic multiplier由同指PIP曲线派生。
+URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch`，共12个，
+其中`thumb_mcp`字段固定为0而不会修改源URDF；
+四指PIP、`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容，
+当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、
+轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
+相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入
+数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测
+前后轴向稳定相差11.4°），因此跨视角方向差超过0.75°融合门限时不再判定任务
+失败：程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的
+前视轴向；仅当差值超过粗错误兜底上限（`cross_view_roll_maximum_axis_difference_deg`
+默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm，
+对应标签贴错连杆或标签松动）时才失败。前视侧摆连杆与侧视PIP连杆的轴线
+本身存在约21mm的系统性位置差（丝杠平移连杆），属预期现象。侧视校验通道
+（`*_mcp_roll_side`）的
+分支间隙跨轮极差上限放宽为
+`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°（绝对间隙1.5°上限
+不变），其`axis_pose_line_rms`降为诊断，不再作为准入门限；径向、平面、
+圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受
+姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限；
+径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
+遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。
+8个组合姿态仅保留为开发诊断，正式产品默认不执行。15-Tag布局的侧面每根手指只有一张
+PIP Tag，并不存在可独立验证的DIP Tag；同时轴线零位求解不提供适合绝对笛卡尔位置验收的
+手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
+`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。
+现场需要区分某根手指的roll机构回差与单机位误差时，可设置
+`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
+正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务，
+双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果
+一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
+锁定URDF发布，不能用`resume`转换成正式标定。
+schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅
+`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动
+方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。
+schema v4继续兼容旧
+命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。
+
 ## 三机位三维关节轴零位标定（schema v4）
 
 正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
@@ -177,7 +308,7 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
 
 前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
 差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
-95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
+训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
 
 单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
 耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
@@ -209,18 +340,22 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
 
 生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
 `origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
-JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
+JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。
 
 坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
 重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
 端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
 连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
 暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
-完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
+完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
-拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
+预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算
+（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到
+后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前
+关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
 只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
 失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
-仍保存在 `raw_samples.jsonl`。
+仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
+PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。
 
 每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
 程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
@@ -273,20 +408,23 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
 ```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
+  meshes/*.STL
 
 calibration_output/G20_RIGHT_001/<时间戳>/
   g20_right_G20_RIGHT_001_calibration.json
-src/.../g20_right/
   linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
+  meshes/*.STL
 ```
 
 文件包含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)、
+改变；当前15-Tag右手保留12个主动静态零位字段，其中11个由数据求解，`thumb_mcp`
+固定为原始CAD零位0；四指PIP虽保留字段但数值固定为0。
+未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
+同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
 
 完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
@@ -315,7 +453,8 @@ ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.
   calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
 ```
 
-默认订阅 `/cb_right_hand_control_cmd`，发布
+schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅
+`/g20/cb_right_hand_control_cmd`。两者均发布
 `/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
 发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
 因此不会静默退回旧标定。

src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml

@@ -0,0 +1,36 @@
+schema_version: 1
+model: G20
+side: right
+serial_number: G20_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
+  source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a
+  calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
+  calibration_config_sha256: 80758d0a240a7dea1426d4fad688b09eac8360560a1d5870d709b7bb103d3280
+  tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml
+  tag_config_sha256: 7ce50be80dfccb3529c34d5bd601090b9d378e2bf6abcef2d5b93dc6a4da99fd
+
+release:
+  # Each task already contains three training cycles plus an isolated fourth
+  # holdout, so a second complete hardware session duplicates hours of motion.
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml

@@ -16,14 +16,24 @@ g20_calibration:
     normal_calibration_speed: 15
     index_roll_calibration_speed: 5
     index_flex_calibration_speed: 10
+    # 15-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
+    # 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
+    # 始终保持速度5；任一方向采样余量不足也保持原速度。
+    adaptive_formal_speed_enabled: true
+    adaptive_formal_speed_max_scale: 1.5
+    adaptive_formal_speed_minimum_bins: 64
+    adaptive_formal_speed_maximum_bin_gap: 8
     speed_setting_settle_seconds: 0.25
 
     # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
     tag_size_m: 0.016
     repetitions: 3
+    # 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
+    g20_right_19_repetitions: 4
     preflight_frames: 60
     minimum_detection_rate: 0.95
     minimum_detection_hz: 15.0
+    minimum_feedback_hz: 25.0
     maximum_hamming: 0
     minimum_decision_margin: 30.0
     minimum_edge_pixels: 30.0
@@ -36,9 +46,15 @@ g20_calibration:
     pnp_tracker_reset_seconds: 5.0
     # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
     pnp_group_initialization_frames: 8
-    # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
+    # 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
     pnp_group_normal_alignment_scale_deg: 5.0
     pnp_group_maximum_normal_alignment_deg: 15.0
+    # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
+    # 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
+    # 缩放或替代被动IP的自身Tag实测曲线。
+    thumb_ip_pnp_coupling_multiplier: 1.03
+    thumb_ip_pnp_coupling_scale_deg: 3.0
+    thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
     top_pnp_invalid_reset_seconds: 1.0
     # 三维位姿必须与实测20通道状态严格按时间戳配对。
     maximum_state_image_skew_ms: 50.0
@@ -54,17 +70,22 @@ g20_calibration:
     # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
     axis_maximum_rotation_circle_difference_deg: 1.0
     # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
-    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
+    # 关节允许可重复的非理想单轴分量，但仍须通过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_observability_condition_number: 10000000000.0
     zero_maximum_offset_deg: 20.0
-    # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
+    # 四指MCP roll/pitch绝对静态零偏的保护范围；PIP仍保留CAD静态零位。
     zero_finger_maximum_offset_deg: 3.0
 
     endpoint_tolerance_u8: 2.0
+    # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
+    # 必须已经稳定；大残差仍由机械卡滞保护处理。
+    steady_checkpoint_command_feedback_tolerance_u8: 8.0
+    steady_checkpoint_maximum_feedback_range_u8: 2.0
     # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
     thumb_yaw_zero_endpoint_tolerance_u8: 4.0
     # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
@@ -72,12 +93,18 @@ g20_calibration:
     # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
     pinky_pip_zero_endpoint_tolerance_u8: 5.0
     endpoint_hold_seconds: 0.5
+    # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
     baseline_hold_seconds: 0.5
+    minimum_baseline_hold_frames: 10
+    # 15-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
+    task_precheck_hold_seconds: 2.0
     position_timeout_seconds: 30.0
     sweep_timeout_seconds: 90.0
-    # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
+    # 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
     # 这类故障不进入遮挡/超时的三次自动重扫。
-    motor_stall_timeout_seconds: 8.0
+    # 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
+    motor_stall_timeout_seconds: 2.0
+    motor_stall_startup_grace_seconds: 1.0
     motor_stall_minimum_progress_u8: 1.0
     invalid_timeout_seconds: 3.0
     minimum_sweep_frames: 40
@@ -85,15 +112,18 @@ g20_calibration:
     minimum_sweep_bins: 32
     maximum_bin_gap: 16
     # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
-    automatic_sweep_retry_limit: 3
+    automatic_sweep_retry_limit: 2
     # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
     automatic_fit_retry_limit: 2
     automatic_motion_retry_limit: 2
+    # 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
+    # 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
+    cross_view_roll_diagnostic_finger: ""
     # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
     provisional_warning_ratio: 1.25
     retry_minimum_speed: 3
-    retry_speed_scales: [0.8, 0.6, 0.5]
+    retry_speed_scales: [0.8, 0.6]
-    retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
+    retry_endpoint_hold_seconds: [0.75, 1.0]
 
     trajectory_maximum_plane_rms_m: 0.004
     trajectory_maximum_radial_rms_m: 0.004
@@ -106,15 +136,40 @@ g20_calibration:
     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
+    # 旧布局仍用连续扫描正反程差门限；15-Tag产品的连续运动包含速度相关滞后，
+    # 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
+    # 的九点稳态command_maximum_direction_gap_deg。
+    maximum_hysteresis_deg: 2.0
+    # 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
+    # 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
+    # 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
+    baseline_maximum_hysteresis_deg: 0.5
+    directional_zero_maximum_branch_gap_deg: 2.0
+    directional_zero_maximum_branch_gap_range_deg: 0.3
+    cross_view_roll_maximum_branch_gap_difference_deg: 0.3
     passive_maximum_monotonic_correction_deg: 3.0
-    passive_maximum_hysteresis_deg: 7.5
+    passive_maximum_hysteresis_deg: 2.0
+    # 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
+    # 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
+    command_maximum_direction_gap_deg: 2.0
 
-    # 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
+    # 默认无额外随机动作；15-Tag产品最终一轮始终作为不可关闭的留出验证。
     validation_enabled: false
+    # 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
+    # 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
+    # Developer diagnostic only. The 15-Tag product has one side PIP Tag per
+    # finger and no independent DIP Tag, while the formal fourth sweep cycle
+    # already provides an isolated per-joint holdout.
+    combination_validation_enabled: false
+    combination_validation_frames: 10
+    combination_maximum_position_p95_m: 0.003
+    combination_maximum_orientation_p95_deg: 2.0
     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
+    # 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
+    maximum_validation_error_deg: 3.0
+    zero_maximum_confidence_half_width_deg: 1.5

src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.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, 11, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 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, 15, 17]
+      frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
+      sizes: [0.016, 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/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -1,8 +1,13 @@
-"""Map G20 u8 commands to URDF joint angles using one calibration JSON.
+"""Map G20 u8 feedback to URDF joint angles using one 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.
+
+Schema-v5 trajectories are fitted against timestamp-synchronised hardware
+feedback, not controller set-points.  They must therefore be queried with the
+SDK ``hand_state`` topic.  The retained schema-v4 path is command-indexed for
+backwards compatibility only.
 """
 
 from __future__ import annotations
@@ -16,7 +21,11 @@ import rclpy
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 
-from .full_hand import get_hand_calibration_profile, validate_compact_payload
+from .full_hand import (
+    get_hand_calibration_profile,
+    infer_compact_payload_layout,
+    validate_compact_payload,
+)
 
 
 G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -71,7 +80,7 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
 
 
 class CalibratedCommandMapper:
-    """Validated, side-specific lookup from a G20 command to URDF radians."""
+    """Validated, side-specific lookup from G20 u8 values to URDF radians."""
 
     def __init__(
         self, payload: Mapping[str, Any], *, expected_side: str | None = None
@@ -86,9 +95,20 @@ class CalibratedCommandMapper:
         quality = payload["quality"]
         if quality.get("passed") is not True:
             raise ValueError("calibration quality.passed must be true")
-        profile = get_hand_calibration_profile(side)
+        layout_id = infer_compact_payload_layout(payload)
+        profile = get_hand_calibration_profile(side, layout_id)
         self.side = side
+        self.layout_id = layout_id
         self.serial_number = str(payload["serial_number"])
+        schema_version = int(payload["schema_version"])
+        self.input_domain = str(
+            payload.get(
+                "curve_input_domain",
+                "command_u8" if schema_version == 4 else "",
+            )
+        )
+        if self.input_domain not in {"command_u8", "feedback_u8"}:
+            raise ValueError("calibration curve_input_domain is invalid")
         self._motor_by_joint = {
             name: int(profile.joint_specs[name].motor_index)
             for name in G20_URDF_JOINT_NAMES
@@ -100,6 +120,27 @@ class CalibratedCommandMapper:
             )
             for name in G20_URDF_JOINT_NAMES
         }
+        self._decreasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "decreasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in G20_URDF_JOINT_NAMES
+        }
+        self._increasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "increasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in G20_URDF_JOINT_NAMES
+        }
+        self._previous_by_motor: dict[int, float] = {}
+        self._direction_by_motor: dict[int, str] = {}
+        self.direction_deadband_u8 = 0.5
 
     @staticmethod
     def _command_index(value: float) -> int:
@@ -133,10 +174,34 @@ class CalibratedCommandMapper:
                 )
             command = values
         indices = tuple(self._command_index(value) for value in command)
-        return tuple(
+        direction_by_motor: dict[int, str | None] = {}
-            self._curves[name][indices[self._motor_by_joint[name]]]
+        for motor, value in enumerate(command):
-            for name in G20_URDF_JOINT_NAMES
+            previous = self._previous_by_motor.get(motor)
-        )
+            direction = self._direction_by_motor.get(motor)
+            if previous is not None:
+                if value > previous + self.direction_deadband_u8:
+                    direction = "increasing"
+                elif value < previous - self.direction_deadband_u8:
+                    direction = "decreasing"
+            direction_by_motor[motor] = direction
+        result: list[float] = []
+        for name in G20_URDF_JOINT_NAMES:
+            motor = self._motor_by_joint[name]
+            direction = direction_by_motor[motor]
+            curves = (
+                self._increasing_curves
+                if direction == "increasing"
+                else self._decreasing_curves
+                if direction == "decreasing"
+                else self._curves
+            )
+            result.append(curves[name][indices[motor]])
+        for motor, value in enumerate(command):
+            self._previous_by_motor[motor] = value
+            direction = direction_by_motor[motor]
+            if direction is not None:
+                self._direction_by_motor[motor] = direction
+        return tuple(result)
 
 
 def load_calibrated_command_mapper(
@@ -149,6 +214,17 @@ def load_calibrated_command_mapper(
     return CalibratedCommandMapper(payload, expected_side=expected_side)
 
 
+def default_input_topic(hand_type: str, input_domain: str) -> str:
+    side = str(hand_type).lower()
+    if side not in {"left", "right"}:
+        raise ValueError("hand_type must be left or right")
+    if input_domain == "feedback_u8":
+        return f"/g20/cb_{side}_hand_state"
+    if input_domain == "command_u8":
+        return f"/g20/cb_{side}_hand_control_cmd"
+    raise ValueError("calibration curve_input_domain is invalid")
+
+
 class CalibratedJointStateBridge(Node):
     def __init__(self) -> None:
         super().__init__("g20_calibrated_joint_state_bridge")
@@ -168,7 +244,9 @@ class CalibratedJointStateBridge(Node):
         )
         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.input_topic = input_topic or default_input_topic(
+            hand_type, self.mapper.input_domain
+        )
         self.output_topic = (
             output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
         )
@@ -179,7 +257,8 @@ class CalibratedJointStateBridge(Node):
         self._last_error = ""
         self.get_logger().info(
             f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
-            f"{self.input_topic} -> {self.output_topic}"
+            f"{self.input_topic} ({self.mapper.input_domain}) -> "
+            f"{self.output_topic}"
         )
 
     def _command_callback(self, command: JointState) -> None:

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py

@@ -0,0 +1,553 @@
+"""One-command product runner for G20_RIGHT_001 calibration."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime
+import json
+import os
+from pathlib import Path
+import signal
+import subprocess
+import sys
+import time
+import traceback
+from typing import Any, Mapping
+
+from ament_index_python.packages import get_package_share_directory
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+
+from .hikrobot_camera import configure_fastdds_large_image_transport
+from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
+from .product import ProductConfig, load_product_config, sha256_file
+from .publication import atomic_session_pointer, finalize_session_artifacts
+
+
+EXIT_PASS = 0
+EXIT_QUALITY = 2
+EXIT_SAFETY = 3
+STATUS_TIMEOUT_SECONDS = 90.0
+
+
+class CalibrationMonitor(Node):
+    def __init__(self) -> None:
+        super().__init__("g20_calibration_product_runner")
+        self.latest_status: dict[str, Any] = {}
+        self.last_status_at = time.monotonic()
+        self.start_requested = False
+        self.start_future: Any = None
+        self.abort_future: Any = None
+        self.create_subscription(String, "/g20_calibration/status", self._status, 10)
+        self.start_client = self.create_client(Trigger, "/g20_calibration/start")
+        self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
+
+    def _status(self, message: String) -> None:
+        try:
+            payload = json.loads(message.data)
+        except (TypeError, json.JSONDecodeError):
+            return
+        if isinstance(payload, dict):
+            self.latest_status = payload
+            self.last_status_at = time.monotonic()
+
+    def maybe_start(self) -> None:
+        if self.start_requested or self.latest_status.get("state") != "WAIT_START":
+            return
+        if not self.start_client.service_is_ready():
+            self.start_client.wait_for_service(timeout_sec=0.05)
+            return
+        self.start_requested = True
+        self.start_future = self.start_client.call_async(Trigger.Request())
+
+    def abort(self) -> None:
+        if not self.abort_client.service_is_ready():
+            self.abort_client.wait_for_service(timeout_sec=1.0)
+        if self.abort_client.service_is_ready():
+            self.abort_future = self.abort_client.call_async(Trigger.Request())
+
+
+class ProgressConsole:
+    def __init__(self, serial_number: str) -> None:
+        self.serial_number = serial_number
+        self.estimator = ProgressEstimator.start()
+        self.last_text = ""
+        self.last_issue = ""
+
+    def update(self, status: Mapping[str, Any]) -> None:
+        text = render_progress_zh(self.serial_number, status, self.estimator)
+        if text == self.last_text:
+            return
+        self.last_text = text
+        if sys.stdout.isatty():
+            sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
+            sys.stdout.flush()
+        else:
+            print(text, flush=True)
+        reason = str(status.get("reason", ""))
+        if reason.startswith("automatic_retry_") and reason != self.last_issue:
+            self.last_issue = reason
+            active = status.get("active", {})
+            print(
+                "\n".join(
+                    [
+                        f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}",
+                        f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）",
+                    ]
+                ),
+                flush=True,
+            )
+
+
+def _default_product_config() -> Path:
+    try:
+        installed = Path(
+            get_package_share_directory("g20_thumb_apriltag_calibration")
+        ) / "config" / "g20_right_product.yaml"
+        if installed.is_file():
+            return installed
+    except Exception:
+        pass
+    return (
+        Path.cwd()
+        / "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
+    ).resolve()
+
+
+def _launch_command(
+    config: ProductConfig,
+    session: Path,
+    *,
+    resume_from: Path | None = None,
+) -> list[str]:
+    values = {
+        "hand_type": "right",
+        "tag_layout": "g20_right_15",
+        "serial_number": config.serial_number,
+        "can_interface": config.can_interface,
+        "session_dir": str(session),
+        "output_root": str(config.output_root),
+        "camera_extrinsics_file": str(config.camera_extrinsics),
+        "source_urdf_path": str(config.source_urdf),
+        "source_urdf_expected_sha256": config.source_urdf_sha256,
+        "corrected_urdf_output_dir": str(session),
+        "calibration_config": str(config.calibration_config),
+        "tag_config": str(config.tag_config),
+        "commands_enabled": "true",
+        "start_cameras": "true",
+        "start_sdk": "true",
+        "record_bag": "false",
+        "validation_enabled": "false",
+    }
+    if resume_from is not None:
+        values["resume_raw_samples_path"] = str(
+            resume_from / "raw_samples.jsonl"
+        )
+    for view, camera in config.cameras.items():
+        values[f"{view}_camera_serial"] = camera["serial_number"]
+        values[f"{view}_camera_name"] = camera["camera_name"]
+        values[f"{view}_camera_info_url"] = camera["camera_info"]
+    return [
+        "ros2",
+        "launch",
+        "g20_thumb_apriltag_calibration",
+        "three_camera_calibration.launch.py",
+        *(f"{name}:={value}" for name, value in values.items()),
+    ]
+
+
+def _stop_stack(process: subprocess.Popen[Any]) -> None:
+    if process.poll() is not None:
+        return
+    try:
+        os.killpg(process.pid, signal.SIGINT)
+    except ProcessLookupError:
+        return
+    try:
+        process.wait(timeout=15.0)
+    except subprocess.TimeoutExpired:
+        try:
+            os.killpg(process.pid, signal.SIGTERM)
+        except ProcessLookupError:
+            return
+        try:
+            process.wait(timeout=5.0)
+        except subprocess.TimeoutExpired:
+            try:
+                os.killpg(process.pid, signal.SIGKILL)
+            except ProcessLookupError:
+                return
+            process.wait(timeout=5.0)
+
+
+def _write_trace(log_path: Path, error: BaseException) -> None:
+    with log_path.open("a", encoding="utf-8") as stream:
+        stream.write("\n[one-command exception]\n")
+        traceback.print_exception(type(error), error, error.__traceback__, file=stream)
+
+
+def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
+    monitor.abort()
+    deadline = time.monotonic() + float(timeout_seconds)
+    while time.monotonic() < deadline and rclpy.ok():
+        rclpy.spin_once(monitor, timeout_sec=0.1)
+        if monitor.latest_status.get("state") == "ABORTED":
+            return
+
+
+def _run_hardware_session(
+    config: ProductConfig,
+    session: Path,
+    *,
+    resume_from: Path | None = None,
+) -> tuple[dict[str, Any], int]:
+    session.mkdir(parents=True, exist_ok=False)
+    (session / "raw_samples.jsonl").touch()
+    log_path = session / "calibration.log"
+    log_stream = log_path.open("a", encoding="utf-8", buffering=1)
+    atomic_session_pointer(config.session_root, "latest_attempt", session)
+    monitor = CalibrationMonitor()
+    console = ProgressConsole(config.serial_number)
+    process: subprocess.Popen[Any] | None = None
+    latest_status: dict[str, Any] = {
+        "state": "PREFLIGHT",
+        "reason": "starting_ros_stack",
+        "progress": 0.0,
+        "views": {},
+        "feedback_hz": 0.0,
+    }
+    exit_code = EXIT_QUALITY
+    try:
+        process = subprocess.Popen(
+            _launch_command(config, session, resume_from=resume_from),
+            cwd=config.workspace,
+            stdout=log_stream,
+            stderr=subprocess.STDOUT,
+            text=True,
+            start_new_session=True,
+        )
+        launched_at = time.monotonic()
+        last_render = 0.0
+        while True:
+            rclpy.spin_once(monitor, timeout_sec=0.1)
+            if monitor.latest_status:
+                latest_status = monitor.latest_status
+                monitor.maybe_start()
+            now = time.monotonic()
+            if now - last_render >= 0.5:
+                console.update(latest_status)
+                last_render = now
+            if monitor.start_future is not None and monitor.start_future.done():
+                response = monitor.start_future.result()
+                if response is None or not response.success:
+                    message = "start service failed" if response is None else response.message
+                    raise RuntimeError(f"CFG-START-008:{message}")
+                monitor.start_future = None
+            state = str(latest_status.get("state", ""))
+            if state == "COMPLETE":
+                exit_code = EXIT_PASS
+                break
+            if state in {"PAUSED", "ABORTED"}:
+                reason = str(latest_status.get("reason", "calibration_paused"))
+                exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
+                if state == "PAUSED" and "stall" not in reason:
+                    # Ordinary quality failures return to the reviewed baseline
+                    # before the process tree is stopped.  Mechanical stalls
+                    # deliberately skip this path and keep the current pose.
+                    failure_status = dict(latest_status)
+                    _request_safe_abort(monitor)
+                    latest_status = failure_status
+                break
+            if process.poll() is not None:
+                raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
+            if (
+                not monitor.latest_status
+                and now - launched_at > STATUS_TIMEOUT_SECONDS
+            ):
+                raise RuntimeError("CAM-STATUS-202:no calibration status received")
+            if (
+                monitor.latest_status
+                and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS
+            ):
+                raise RuntimeError("MOTION-COMM-303:calibration status stopped")
+    except KeyboardInterrupt as error:
+        latest_status["state"] = "ABORTED"
+        latest_status["reason"] = "operator_abort"
+        _request_safe_abort(monitor)
+        _write_trace(log_path, error)
+        exit_code = EXIT_SAFETY
+    except BaseException as error:
+        latest_status["state"] = "PAUSED"
+        latest_status["reason"] = str(error)
+        _write_trace(log_path, error)
+        exit_code = EXIT_QUALITY
+    finally:
+        if process is not None:
+            _stop_stack(process)
+        monitor.destroy_node()
+        log_stream.flush()
+        os.fsync(log_stream.fileno())
+        log_stream.close()
+
+    if exit_code != EXIT_PASS:
+        _, block = build_failure_report(
+            config,
+            session,
+            latest_status,
+            reason=str(latest_status.get("reason", "unknown_failure")),
+        )
+        print(block, flush=True)
+    return latest_status, exit_code
+
+
+def _startup_failure_block(path: Path, error: BaseException) -> str:
+    return "\n".join(
+        [
+            "========== 请复制以下内容给开发者 ==========",
+            "结果：FAIL",
+            "错误代码：CFG-PRODUCT-001",
+            "失败阶段：启动静态预检",
+            f"问题：{error}",
+            f"产品配置：{path}",
+            "自动处理：未启动相机、SDK或机械手运动",
+            "建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。",
+            "========== 复制结束 ==========",
+        ]
+    )
+
+
+def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
+    """Return the newest compatible failed attempt, never a passed session.
+
+    Do not trust only ``latest_attempt``.  A process interrupted during the
+    device-only startup gate may have already moved that pointer while still
+    containing no ``session_start`` checkpoint.  In that case walk backwards
+    to the preceding usable failed session instead of throwing away hours of
+    completed tasks.
+    """
+    root = config.session_root
+    try:
+        resolved_root = root.resolve(strict=True)
+    except OSError:
+        return None
+
+    candidates: list[Path] = []
+    pointer = config.session_root / "latest_attempt"
+    if pointer.exists():
+        try:
+            candidates.append(pointer.resolve(strict=True))
+        except OSError:
+            pass
+    try:
+        candidates.extend(
+            sorted(
+                (
+                    path
+                    for path in root.iterdir()
+                    if path.is_dir() and not path.name.startswith("latest_")
+                ),
+                key=lambda path: path.name,
+                reverse=True,
+            )
+        )
+    except OSError:
+        return None
+
+    passed_pointer = config.session_root / "latest_passed"
+    passed: Path | None = None
+    if passed_pointer.exists():
+        try:
+            passed = passed_pointer.resolve(strict=True)
+        except OSError:
+            pass
+
+    seen: set[Path] = set()
+    for unresolved in candidates:
+        try:
+            candidate = unresolved.resolve(strict=True)
+        except OSError:
+            continue
+        if candidate in seen:
+            continue
+        seen.add(candidate)
+        if candidate.parent != resolved_root or not candidate.is_dir():
+            continue
+        # A failed attempt older than the current formal release is stale and
+        # must not seed a new independent calibration.
+        if passed is not None and candidate.name <= passed.name:
+            continue
+        raw_path = candidate / "raw_samples.jsonl"
+        if not raw_path.is_file():
+            continue
+        summary_path = candidate / "calibration_summary_zh.json"
+        summary: dict[str, Any] | None = None
+        if summary_path.is_file():
+            try:
+                loaded = json.loads(summary_path.read_text(encoding="utf-8"))
+            except (OSError, json.JSONDecodeError):
+                continue
+            if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
+                continue
+            summary = loaded
+            hashes = summary.get("hashes", {})
+            if not isinstance(hashes, Mapping):
+                continue
+            if (
+                str(hashes.get("source_urdf_sha256", ""))
+                != config.source_urdf_sha256
+                or str(hashes.get("camera_extrinsics_sha256", ""))
+                != config.camera_extrinsics_sha256
+            ):
+                continue
+        start: dict[str, Any] | None = None
+        try:
+            with raw_path.open("r", encoding="utf-8") as stream:
+                for line in stream:
+                    if not line.strip():
+                        continue
+                    value = json.loads(line)
+                    if (
+                        isinstance(value, dict)
+                        and value.get("kind") == "session_start"
+                    ):
+                        start = value
+                        break
+        except (OSError, json.JSONDecodeError):
+            continue
+        if (
+            start is None
+            or start.get("hand_type") != "right"
+            or start.get("tag_layout") != "g20_right_15"
+            or start.get("source_urdf_sha256")
+            != config.source_urdf_sha256
+        ):
+            continue
+        if summary is None:
+            # Ctrl+C can terminate the ROS launch tree before the wrapper gets
+            # a chance to create calibration_summary_zh.json.  The immutable
+            # checkpoint itself is enough to resume only after independently
+            # proving that its external geometry still matches the product.
+            try:
+                checkpoint_extrinsics = Path(
+                    str(start["camera_extrinsics_file"])
+                ).expanduser().resolve(strict=True)
+                if sha256_file(checkpoint_extrinsics) != (
+                    config.camera_extrinsics_sha256
+                ):
+                    continue
+            except (KeyError, OSError, ValueError):
+                continue
+        return candidate
+    return None
+
+
+def run(
+    config_path: str | Path,
+    *,
+    workspace: str | Path | None = None,
+    preflight_only: bool = False,
+    allow_resume: bool = True,
+) -> int:
+    path = Path(config_path).expanduser().resolve()
+    try:
+        # Resolve every file and camera identity before allowing a hardware
+        # process to start.  A second load enables the real CAN existence gate.
+        config = load_product_config(path, workspace=workspace, check_can=False)
+        load_product_config(path, workspace=workspace, check_can=True)
+    except BaseException as error:
+        print(_startup_failure_block(path, error), flush=True)
+        return EXIT_QUALITY
+    if preflight_only:
+        print("PASS：产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。")
+        return EXIT_PASS
+
+    config.session_root.mkdir(parents=True, exist_ok=True)
+    resume_candidate = (
+        _automatic_resume_candidate(config) if allow_resume else None
+    )
+    if resume_candidate is not None:
+        print(
+            "检测到兼容的失败会话，将恢复已完整通过的关节任务："
+            f"{resume_candidate.name}。失败中的当前任务会从头重做。",
+            flush=True,
+        )
+    maximum_sessions = config.required_independent_passes
+    for pass_index in range(maximum_sessions):
+        stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        session = config.session_root / stamp
+        while session.exists():
+            time.sleep(1.0)
+            stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+            session = config.session_root / stamp
+        if maximum_sessions > 1:
+            print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
+        status, code = _run_hardware_session(
+            config,
+            session,
+            resume_from=(resume_candidate if pass_index == 0 else None),
+        )
+        if code != EXIT_PASS:
+            return code
+        try:
+            summary, release_ready = finalize_session_artifacts(
+                config, session, node_status=status
+            )
+        except BaseException as error:
+            _write_trace(session / "calibration.log", error)
+            status = dict(status)
+            status["state"] = "PAUSED"
+            status["reason"] = f"PUB-ARTIFACT-601:{error}"
+            _, block = build_failure_report(config, session, status, reason=status["reason"])
+            print(block, flush=True)
+            return EXIT_QUALITY
+        if release_ready:
+            print(
+                "\n".join(
+                    [
+                        "PASS：G20右手标定、URDF修正和独立复验全部通过。",
+                        f"正式结果：{config.session_root / 'latest_passed'}",
+                        f"JSON：{session / f'g20_right_{config.serial_number}_calibration.json'}",
+                        f"URDF：{summary['artifacts']['urdf']}",
+                    ]
+                ),
+                flush=True,
+            )
+            return EXIT_PASS
+        print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True)
+    return EXIT_QUALITY
+
+
+def main(args: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(description="G20右手一键精密标定")
+    parser.add_argument("--config", default=str(_default_product_config()))
+    parser.add_argument("--workspace", default=None)
+    parser.add_argument("--preflight-only", action="store_true")
+    parser.add_argument(
+        "--no-resume",
+        action="store_true",
+        help="忽略失败会话，从第一个关节开始全新采集",
+    )
+    arguments = parser.parse_args(args)
+    configure_fastdds_large_image_transport()
+    ros_log_dir = Path(
+        os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
+    )
+    ros_log_dir.mkdir(parents=True, exist_ok=True)
+    rclpy.init()
+    try:
+        code = run(
+            arguments.config,
+            workspace=arguments.workspace,
+            preflight_only=arguments.preflight_only,
+            allow_resume=not arguments.no_resume,
+        )
+    finally:
+        if rclpy.ok():
+            rclpy.shutdown()
+    raise SystemExit(code)
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py

@@ -0,0 +1,394 @@
+"""Stable Chinese progress and copy/paste diagnostics for non-expert users."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import json
+import math
+from pathlib import Path
+import time
+from typing import Any, Mapping
+
+from .product import ProductConfig
+from .storage import atomic_write_json
+from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh
+
+
+def _duration(seconds: float | None) -> str:
+    if seconds is None or not math.isfinite(seconds) or seconds < 0.0:
+        return "计算中"
+    value = int(round(seconds))
+    return f"{value // 60}分{value % 60:02d}秒"
+
+
+def _task_tag_id_status(views: Mapping[str, Any]) -> str:
+    """Render every currently required Tag ID with its live visibility."""
+    labels = {"front": "正面", "side": "侧面", "top": "顶部"}
+    ordered_views = [
+        *[name for name in ("front", "side", "top") if name in views],
+        *sorted(name for name in views if name not in labels),
+    ]
+    groups: list[str] = []
+    for name in ordered_views:
+        item = views.get(name, {})
+        if not isinstance(item, Mapping):
+            continue
+        required = sorted({int(value) for value in item.get("required_tag_ids", [])})
+        if not required:
+            continue
+        visible = {int(value) for value in item.get("detected_tag_ids", [])}
+        locked = {
+            int(value)
+            for value in item.get("locked_reference_tag_ids", [])
+        }
+        ids = ",".join(
+            f"{tag_id}{'锁' if tag_id in locked else '✓' if tag_id in visible else '✗'}"
+            for tag_id in required
+        )
+        groups.append(f"{labels.get(name, name)}[{ids}]")
+    if not groups:
+        return ""
+    return (
+        "  当前任务ID："
+        + " ".join(groups)
+        + "（✓实时可见/锁=基准锁定/✗不可用）"
+    )
+
+
+@dataclass
+class ProgressEstimator:
+    started_at: float
+    last_progress: float = 0.0
+    last_progress_at: float = 0.0
+    seconds_per_fraction: float | None = None
+
+    @classmethod
+    def start(cls) -> "ProgressEstimator":
+        now = time.monotonic()
+        return cls(started_at=now, last_progress_at=now)
+
+    def remaining(self, progress: float, now: float | None = None) -> float | None:
+        current = time.monotonic() if now is None else float(now)
+        value = float(max(0.0, min(1.0, progress)))
+        delta = value - self.last_progress
+        elapsed = current - self.last_progress_at
+        if delta >= 0.002 and elapsed > 0.0:
+            estimate = elapsed / delta
+            self.seconds_per_fraction = (
+                estimate
+                if self.seconds_per_fraction is None
+                else 0.8 * self.seconds_per_fraction + 0.2 * estimate
+            )
+            self.last_progress = value
+            self.last_progress_at = current
+        if value >= 1.0:
+            return 0.0
+        if self.seconds_per_fraction is not None:
+            return max(0.0, self.seconds_per_fraction * (1.0 - value))
+        if value >= 0.02:
+            return max(0.0, (current - self.started_at) * (1.0 - value) / value)
+        return None
+
+
+def render_progress_zh(
+    serial_number: str,
+    status: Mapping[str, Any],
+    estimator: ProgressEstimator,
+) -> str:
+    progress = float(status.get("progress", 0.0))
+    active = status.get("active", {})
+    if not isinstance(active, Mapping):
+        active = {}
+    state = str(status.get("state", "PREFLIGHT"))
+    stage = STATE_NAMES_ZH.get(state, state)
+    preflight_mode = str(status.get("preflight_mode", ""))
+    if preflight_mode == "device_only_before_baseline":
+        stage = "设备连接预检"
+    elif preflight_mode == "recovering_baseline":
+        stage = "安全恢复基准形态"
+    elif preflight_mode == "baseline_tags_after_recovery":
+        stage = "基准姿态标签预检"
+    elif str(status.get("reason", "")) == (
+        "holding_same_finger_clearance_before_next_task"
+    ):
+        stage = "保持避让姿态，切换同指下一项"
+    elif str(status.get("reason", "")) == (
+        "waiting_for_task_tags_at_sweep_start"
+    ):
+        stage = "已到扫描起点，等待任务Tag"
+    joints = active.get("joints", [])
+    joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints)
+    if str(active.get("task_name", "")).endswith("_roll_multiview") and joints:
+        joint = (
+            JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0]))
+            + "（正面+侧面同步）"
+        )
+    if not joint:
+        joint = str(active.get("label_zh", "")) or (
+            "等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
+        )
+    direction = {
+        "decreasing": "递减",
+        "increasing": "递增",
+    }.get(str(active.get("direction", "")), "-")
+    cycle = active.get("cycle", "-")
+    repetitions = active.get("repetitions", 4)
+    requested = active.get(
+        "current_motion_target_u8",
+        active.get("command_u8", active.get("pose_name", "-")),
+    )
+    actual = active.get("actual_u8", "-")
+    views = status.get("views", {})
+    if not isinstance(views, Mapping):
+        views = {}
+    ready_cameras = sum(
+        bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid"))
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    required_tags = sum(
+        len(item.get("required_tag_ids", []))
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    visible_tags = sum(
+        len(
+            {
+                *item.get("detected_tag_ids", []),
+                *item.get("locked_reference_tag_ids", []),
+            }
+        )
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    locked_tag_count = sum(
+        len(item.get("locked_reference_tag_ids", []))
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    configured_tags = sum(
+        len(item.get("configured_tag_ids", item.get("required_tag_ids", [])))
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    visible_configured_tags = sum(
+        len(
+            item.get(
+                "visible_configured_tag_ids",
+                item.get("detected_tag_ids", []),
+            )
+        )
+        for item in views.values()
+        if isinstance(item, Mapping)
+    )
+    occlusion_allowed = bool(
+        state in {"PREFLIGHT", "WAIT_START"}
+        or active.get("pose_name")
+    )
+    if preflight_mode == "device_only_before_baseline":
+        tag_status = (
+            f"当前 {visible_configured_tags}/{configured_tags}"
+            "（基准恢复后检查）"
+        )
+    elif preflight_mode == "recovering_baseline":
+        tag_status = (
+            f"当前 {visible_configured_tags}/{configured_tags}"
+            "（恢复中不作为门槛）"
+        )
+    elif occlusion_allowed:
+        tag_status = (
+            f"当前 {visible_configured_tags}/{configured_tags}（允许遮挡）  "
+            f"本阶段必需 {visible_tags}/{required_tags}"
+        )
+    else:
+        tag_status = (
+            f"{visible_tags}/{required_tags} 有效"
+            + (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "")
+        )
+    task_tag_ids = (
+        _task_tag_id_status(views)
+        if state
+        in {
+            "PREPARE_SWEEP",
+            "SWEEP",
+            "VALIDATION_MOVE",
+            "VALIDATION_CAPTURE",
+        }
+        else ""
+    )
+    retry = int(active.get("automatic_retry_count", 0))
+    fit_attempt = int(active.get("fit_attempt", 1))
+    fit_attempt_limit = int(active.get("fit_attempt_limit", 1))
+    feedback_hz = float(status.get("feedback_hz", 0.0))
+    eta = (
+        "等待Tag"
+        if str(status.get("reason", ""))
+        == "waiting_for_task_tags_at_sweep_start"
+        else _duration(estimator.remaining(progress))
+    )
+    lines = [
+            f"[{serial_number}] 标定中  {progress * 100:5.1f}%  预计剩余 {eta}",
+            f"阶段：{stage}（第 {cycle}/{repetitions} 轮）",
+            f"任务：{joint}  命令/反馈：{requested}/{actual}  方向：{direction}",
+            f"Tag：{tag_status}{task_tag_ids}  相机：{ready_cameras}/3 正常  反馈：{feedback_hz:.1f} Hz",
+            f"质量：有效帧 {active.get('valid_frames', 0)}  已自动重扫 {retry} 次",
+    ]
+    if fit_attempt_limit > 1:
+        lines.append(
+            f"拟合：整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试；"
+            "每次包含完整四轮双向扫描"
+        )
+    resume = status.get("resume", {})
+    if isinstance(resume, Mapping) and resume.get("used"):
+        lines.append(
+            "断点：已恢复 "
+            f"{int(resume.get('completed_task_count', 0))}/"
+            f"{int(resume.get('total_task_count', 16))} 个完整任务；"
+            "失败任务已丢弃并重新采集"
+        )
+    return "\n".join(lines)
+
+
+def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]:
+    value = str(reason)
+    active = status.get("active", {})
+    if not isinstance(active, Mapping):
+        active = {}
+    views = status.get("views", {})
+    missing = [
+        tag
+        for item in views.values()
+        if isinstance(item, Mapping)
+        for tag in item.get("missing_tag_ids", [])
+    ] if isinstance(views, Mapping) else []
+    group_pnp_reasons = [
+        str(item.get("group_pnp_reason"))
+        for item in views.values()
+        if isinstance(item, Mapping) and item.get("group_pnp_reason")
+    ] if isinstance(views, Mapping) else []
+    if value.startswith("CFG-"):
+        return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。"
+    if "motor_state_stalled" in value:
+        return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。"
+    if any(
+        token in value
+        for token in (
+            "sweep_missing_endpoint_bin",
+            "sweep_bins_too_few",
+            "sweep_bin_gap_too_large",
+            "task_precheck_missing_command_127",
+        )
+    ):
+        return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
+    if "synchronised" in value and not missing and group_pnp_reasons:
+        return "CAM-GEOMETRY-201", "Tag可见，但整组PnP候选持续被几何检查拒绝", "不要调整Tag；复制本诊断块给开发者检查PnP候选选择。"
+    if missing or "tag" in value or "detection" in value or "synchronised" in value:
+        return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
+    if "state" in value and ("timeout" in value or "lost" in value):
+        return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。"
+    if "camera" in value or "pnp" in value or "extrinsics" in value:
+        return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动，然后复制本诊断块给开发者。"
+    if "checkpoint" in value:
+        return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。"
+    if "fit" in value or "trajectory" in value or "axis" in value:
+        return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。"
+    if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
+        return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布；复制本诊断块给开发者。"
+    if "publish" in value or "artifact" in value or "URDF" in value:
+        return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖；复制本诊断块给开发者。"
+    return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者，完整堆栈已写入calibration.log。"
+
+
+def build_failure_report(
+    config: ProductConfig,
+    session: str | Path,
+    status: Mapping[str, Any],
+    *,
+    reason: str,
+) -> tuple[dict[str, Any], str]:
+    directory = Path(session).resolve()
+    code, problem, suggestion = classify_error(reason, status)
+    active = status.get("active", {})
+    if not isinstance(active, Mapping):
+        active = {}
+    else:
+        active = dict(active)
+    views = status.get("views", {})
+    if isinstance(views, Mapping):
+        group_pnp_reasons = {
+            str(view): str(item.get("group_pnp_reason"))
+            for view, item in views.items()
+            if isinstance(item, Mapping) and item.get("group_pnp_reason")
+        }
+        if group_pnp_reasons:
+            active["group_pnp_reasons"] = group_pnp_reasons
+    try:
+        explanation, automatic_action = three_camera_reason_zh(
+            str(status.get("state", "")), reason, active
+        )
+    except Exception:
+        explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据"
+    camera_state = {
+        view: (
+            "OK"
+            if isinstance(item, Mapping)
+            and item.get("camera_info_valid", item.get("ready"))
+            and item.get("camera_extrinsics_valid", item.get("ready"))
+            and item.get("stream_alive", item.get("ready"))
+            else "NOT_READY"
+        )
+        for view, item in (views.items() if isinstance(views, Mapping) else [])
+    }
+    session_id = f"{config.serial_number}_{directory.name}"
+    metrics = {
+        "valid_frames": active.get("valid_frames"),
+        "sample": active.get("sample", {}),
+        "automatic_retry_count": active.get("automatic_retry_count", 0),
+    }
+    payload: dict[str, Any] = {
+        "schema_version": 1,
+        "serial_number": config.serial_number,
+        "session_id": session_id,
+        "result": "FAIL",
+        "error_code": code,
+        "stage": str(status.get("state", "startup")),
+        "reason": str(reason),
+        "problem_zh": problem,
+        "explanation_zh": explanation,
+        "automatic_action_zh": automatic_action,
+        "suggestion_zh": suggestion,
+        "metrics": metrics,
+        "camera_state": camera_state,
+        "feedback_hz": status.get("feedback_hz", 0.0),
+        "hashes": {
+            "product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(),
+            "camera_extrinsics_sha256": config.camera_extrinsics_sha256,
+            "calibration_config_sha256": config.calibration_config_sha256,
+            "source_urdf_sha256": config.source_urdf_sha256,
+        },
+        "session_dir": str(directory),
+        "quality": {"passed": False},
+    }
+    atomic_write_json(directory / "calibration_summary_zh.json", payload)
+    block = "\n".join(
+        [
+            "========== 请复制以下内容给开发者 ==========",
+            f"会话编号：{session_id}",
+            "结果：FAIL",
+            f"错误代码：{code}",
+            f"失败阶段：{payload['stage']}",
+            f"问题：{problem}",
+            f"详细说明：{explanation}",
+            f"自动处理：{automatic_action}",
+            f"关键指标：{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
+            f"相机状态：{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
+            f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz",
+            f"配置哈希：{payload['hashes']['product_config_sha256']}",
+            f"外参哈希：{config.camera_extrinsics_sha256}",
+            f"源 URDF 哈希：{config.source_urdf_sha256}",
+            f"会话目录：{directory}",
+            f"建议：{suggestion}",
+            "========== 复制结束 ==========",
+        ]
+    )
+    return payload, block

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py

@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
     relative_translation_scale_m: float,
     normal_alignment_pairs: Sequence[tuple[str, str]] = (),
     normal_alignment_scale_rad: float = math.radians(5.0),
+    task_reference_pairs: Mapping[
+        tuple[str, str], tuple[Rotation, np.ndarray]
+    ] | None = None,
+    task_reference_rotation_scale_rad: float = math.radians(1.0),
+    task_reference_translation_scale_m: float = 0.01,
 ) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
     """Select a static multi-Tag IPPE branch path in bounded time.
 
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
     normal_pair_names = tuple(
         (str(first), str(second)) for first, second in normal_alignment_pairs
     )
+    task_references = dict(task_reference_pairs or {})
     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)
+        for parent, child in (
+            *pair_names,
+            *normal_pair_names,
+            *task_references,
+        )
     ):
         raise ValueError("geometry pairs must reference roles")
     scales = (
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
         float(relative_rotation_scale_rad),
         float(relative_translation_scale_m),
         float(normal_alignment_scale_rad),
+        float(task_reference_rotation_scale_rad),
+        float(task_reference_translation_scale_m),
     )
     if min(scales) <= 0.0:
         raise ValueError("static initialization scales must be positive")
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
         relative_rotation_scale,
         relative_translation_scale,
         normal_scale,
+        task_reference_rotation_scale,
+        task_reference_translation_scale,
     ) = scales
 
     combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
                 + float(np.linalg.norm(translation - reference_translation))
                 / relative_translation_scale
             )
+        for pair, (task_rotation, task_translation) in task_references.items():
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            score += (
+                float((task_rotation.inv() * rotation).magnitude())
+                / task_reference_rotation_scale
+                + float(np.linalg.norm(translation - task_translation))
+                / task_reference_translation_scale
+            )
         return float(score)
 
     best_total = float("inf")
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
     quality = dict(quality)
     quality["total_cost"] = float(best_total)
     quality["initialization_search"] = "static_reference"
+    quality["task_reference_used"] = (
+        "true" if task_references else "false"
+    )
     return selected_path, quality
 
 
@@ -1013,6 +1040,13 @@ class SquareTagGroupPoseTracker:
         normal_alignment_pairs: Sequence[tuple[str, str]] = (),
         normal_alignment_scale_rad: float = math.radians(5.0),
         maximum_normal_alignment_rad: float | None = None,
+        return_reference_rotation_scale_rad: float = math.radians(1.0),
+        return_reference_maximum_command_gap_u8: int = 8,
+        coupled_rotation_pairs: Sequence[
+            tuple[str, str, str, str, float]
+        ] = (),
+        coupled_rotation_scale_rad: float = math.radians(3.0),
+        maximum_coupled_rotation_residual_rad: float | None = None,
     ) -> None:
         self.roles = tuple(str(role) for role in roles)
         self.adjacent_pairs = tuple(
@@ -1023,6 +1057,22 @@ class SquareTagGroupPoseTracker:
             (str(first), str(second))
             for first, second in normal_alignment_pairs
         )
+        self.coupled_rotation_pairs = tuple(
+            (
+                str(driver_parent),
+                str(driver_child),
+                str(follower_parent),
+                str(follower_child),
+                float(multiplier),
+            )
+            for (
+                driver_parent,
+                driver_child,
+                follower_parent,
+                follower_child,
+                multiplier,
+            ) in coupled_rotation_pairs
+        )
         if not self.roles or len(set(self.roles)) != len(self.roles):
             raise ValueError("roles must be non-empty and unique")
         if any(
@@ -1054,6 +1104,20 @@ class SquareTagGroupPoseTracker:
             if maximum_normal_alignment_rad is None
             else float(maximum_normal_alignment_rad)
         )
+        self.return_reference_rotation_scale_rad = float(
+            return_reference_rotation_scale_rad
+        )
+        self.return_reference_maximum_command_gap_u8 = int(
+            return_reference_maximum_command_gap_u8
+        )
+        self.coupled_rotation_scale_rad = float(
+            coupled_rotation_scale_rad
+        )
+        self.maximum_coupled_rotation_residual_rad = (
+            None
+            if maximum_coupled_rotation_residual_rad is None
+            else float(maximum_coupled_rotation_residual_rad)
+        )
         reset_seconds = float(reset_after_seconds)
         if min(
             self.maximum_pose_jump_rad,
@@ -1062,6 +1126,8 @@ class SquareTagGroupPoseTracker:
             self.relative_translation_scale_m,
             self.reprojection_scale_px,
             self.normal_alignment_scale_rad,
+            self.return_reference_rotation_scale_rad,
+            self.coupled_rotation_scale_rad,
             reset_seconds,
         ) <= 0.0:
             raise ValueError("group tracking scales must be positive")
@@ -1069,11 +1135,33 @@ class SquareTagGroupPoseTracker:
             raise ValueError("reprojection_weight must be non-negative")
         if self.initialization_frames < 1:
             raise ValueError("initialization_frames must be positive")
+        if self.return_reference_maximum_command_gap_u8 < 0:
+            raise ValueError(
+                "return reference maximum command gap must be non-negative"
+            )
         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")
+        if any(
+            role not in self.roles
+            for coupling in self.coupled_rotation_pairs
+            for role in coupling[:4]
+        ):
+            raise ValueError("coupled rotation pairs must reference roles")
+        if any(
+            multiplier <= 0.0
+            for *_, multiplier in self.coupled_rotation_pairs
+        ):
+            raise ValueError("coupled rotation multipliers must be positive")
+        if (
+            self.maximum_coupled_rotation_residual_rad is not None
+            and self.maximum_coupled_rotation_residual_rad <= 0.0
+        ):
+            raise ValueError(
+                "maximum coupled rotation residual 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
@@ -1083,22 +1171,200 @@ class SquareTagGroupPoseTracker:
         self._initial_stamps_ns: list[int] = []
         self.last_initialization_quality: dict[str, float | str] = {}
         self.branch_correction_counts: dict[str, int] = {}
+        self._decreasing_relative_rotations: dict[
+            int, dict[tuple[str, str], Rotation]
+        ] = {}
+        self._coupled_reference_rotations: dict[
+            tuple[str, str], Rotation
+        ] = {}
+        self._task_reference_relative_poses: dict[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ] = {}
 
-    def reset(self) -> None:
+    def reset(self, *, preserve_task_reference: bool = False) -> 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()
+        self._decreasing_relative_rotations.clear()
+        self._coupled_reference_rotations.clear()
+        if not preserve_task_reference:
+            self._task_reference_relative_poses.clear()
+
+    def _task_reference_cost(
+        self, combination: Mapping[str, SquareTagPose]
+    ) -> float:
+        residual = 0.0
+        for pair, (expected_rotation, expected_translation) in (
+            self._task_reference_relative_poses.items()
+        ):
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            residual += (
+                float((expected_rotation.inv() * rotation).magnitude())
+                / self.return_reference_rotation_scale_rad
+                + float(np.linalg.norm(translation - expected_translation))
+                / self.relative_translation_scale_m
+            )
+        return residual
+
+    def _coupled_rotation_residuals(
+        self, combination: Mapping[str, SquareTagPose]
+    ) -> tuple[float, ...]:
+        if not self.coupled_rotation_pairs:
+            return ()
+        residuals: list[float] = []
+        for (
+            driver_parent,
+            driver_child,
+            follower_parent,
+            follower_child,
+            multiplier,
+        ) in self.coupled_rotation_pairs:
+            driver_pair = (driver_parent, driver_child)
+            follower_pair = (follower_parent, follower_child)
+            if (
+                driver_pair not in self._coupled_reference_rotations
+                or follower_pair not in self._coupled_reference_rotations
+            ):
+                return ()
+            driver_rotation = _relative_pose(
+                combination[driver_parent], combination[driver_child]
+            )[0]
+            follower_rotation = _relative_pose(
+                combination[follower_parent], combination[follower_child]
+            )[0]
+            driver_travel = (
+                self._coupled_reference_rotations[driver_pair].inv()
+                * driver_rotation
+            ).magnitude()
+            follower_travel = (
+                self._coupled_reference_rotations[follower_pair].inv()
+                * follower_rotation
+            ).magnitude()
+            residuals.append(
+                abs(float(follower_travel) - multiplier * float(driver_travel))
+            )
+        return tuple(residuals)
+
+    def _informative_coupled_rotation_costs(
+        self,
+        combinations: Sequence[Mapping[str, SquareTagPose]],
+    ) -> tuple[float, ...]:
+        """Return branch costs only while the weak coupling prior is credible.
+
+        The URDF mimic ratio is useful for distinguishing two planar-IPPE
+        branches, but it is not measurement truth for a passive joint.  Once
+        every otherwise viable combination disagrees with that ratio, using
+        it would bias the measured curve (and previously rejected every
+        frame).  In that case fall back to visual continuity for this frame.
+        """
+        residuals = tuple(
+            self._coupled_rotation_residuals(combination)
+            for combination in combinations
+        )
+        if not residuals or not any(residuals):
+            return tuple(0.0 for _ in combinations)
+        if (
+            self.maximum_coupled_rotation_residual_rad is not None
+            and not any(
+                values
+                and max(values)
+                <= self.maximum_coupled_rotation_residual_rad
+                for values in residuals
+            )
+        ):
+            return tuple(0.0 for _ in combinations)
+        return tuple(
+            sum(values) / self.coupled_rotation_scale_rad
+            for values in residuals
+        )
+
+    def _return_reference(
+        self, command_u8: int | None
+    ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
+        if command_u8 is None or not self._decreasing_relative_rotations:
+            return {}
+        command = int(command_u8)
+        nearest = min(
+            self._decreasing_relative_rotations,
+            key=lambda candidate: abs(candidate - command),
+        )
+        if (
+            abs(nearest - command)
+            > self.return_reference_maximum_command_gap_u8
+        ):
+            return {}
+        references = self._decreasing_relative_rotations[nearest]
+        commands = sorted(self._decreasing_relative_rotations)
+        axes: dict[tuple[str, str], np.ndarray | None] = {}
+        for pair in self.adjacent_pairs:
+            endpoint_delta = (
+                self._decreasing_relative_rotations[commands[-1]][pair].inv()
+                * self._decreasing_relative_rotations[commands[0]][pair]
+            ).as_rotvec()
+            norm = float(np.linalg.norm(endpoint_delta))
+            axes[pair] = (
+                None
+                if norm < math.radians(5.0)
+                else endpoint_delta / norm
+            )
+        return {
+            pair: (rotation, axes[pair])
+            for pair, rotation in references.items()
+        }
+
+    def _return_reference_cost(
+        self,
+        combination: Mapping[str, SquareTagPose],
+        reference: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray | None]
+        ],
+    ) -> float:
+        residual = 0.0
+        for pair, (expected, motion_axis) in reference.items():
+            vector = (
+                expected.inv()
+                * _relative_pose(
+                    combination[pair[0]], combination[pair[1]]
+                )[0]
+            ).as_rotvec()
+            if motion_axis is not None:
+                # The outbound trajectory identifies the physical one-DOF
+                # motion axis.  Do not penalize return travel along that axis:
+                # it may contain real mechanical hysteresis that calibration
+                # must measure.  A planar-IPPE mirror branch appears primarily
+                # as a large orthogonal tilt and is rejected by this residual.
+                vector = vector - motion_axis * float(vector @ motion_axis)
+            residual += float(np.linalg.norm(vector))
+        return residual / self.return_reference_rotation_scale_rad
 
     def select(
         self,
         candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
         *,
         stamp_ns: int,
+        trajectory_command_u8: int | None = None,
+        trajectory_direction: str | None = None,
     ) -> tuple[dict[str, SquareTagPose] | None, str]:
         """Return one mutually consistent pose for every configured role."""
+        direction = (
+            None
+            if trajectory_direction is None
+            else str(trajectory_direction)
+        )
+        if direction not in {None, "decreasing", "increasing"}:
+            raise ValueError(
+                "trajectory_direction must be decreasing or increasing"
+            )
+        return_reference = (
+            self._return_reference(trajectory_command_u8)
+            if direction == "increasing"
+            else {}
+        )
         candidate_lists = [
             tuple(candidates_by_role.get(role, ()))
             for role in self.roles
@@ -1175,6 +1441,15 @@ class SquareTagGroupPoseTracker:
                         normal_alignment_scale_rad=(
                             self.normal_alignment_scale_rad
                         ),
+                        task_reference_pairs=(
+                            self._task_reference_relative_poses
+                        ),
+                        task_reference_rotation_scale_rad=(
+                            self.return_reference_rotation_scale_rad
+                        ),
+                        task_reference_translation_scale_m=(
+                            self.relative_translation_scale_m
+                        ),
                     )
                 )
                 selected = selected_path[-1]
@@ -1195,23 +1470,31 @@ class SquareTagGroupPoseTracker:
                 ):
                     return None, "group_normal_alignment"
             else:
+                coupling_costs = self._informative_coupled_rotation_costs(
+                    combinations
+                )
                 selected = min(
-                    combinations,
+                    zip(combinations, coupling_costs),
-                    key=lambda combination: (
+                    key=lambda item: (
                         sum(
                             pose.reprojection_error_px
-                            for pose in combination.values()
+                            for pose in item[0].values()
                         )
                         / self.reprojection_scale_px
                         + sum(
                             _normal_alignment_rad(
-                                combination[first], combination[second]
+                                item[0][first], item[0][second]
                             )
                             for first, second in self.normal_alignment_pairs
                         )
                         / self.normal_alignment_scale_rad
+                        + self._return_reference_cost(
+                            item[0], return_reference
+                        )
+                        + item[1]
+                        + self._task_reference_cost(item[0])
                     ),
-                )
+                )[0]
                 maximum_alignment = max(
                     (
                         _normal_alignment_rad(
@@ -1238,7 +1521,7 @@ class SquareTagGroupPoseTracker:
                 )
                 for pair in self.adjacent_pairs
             }
-            scored: list[tuple[float, dict[str, SquareTagPose]]] = []
+            base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
             for combination in combinations:
                 absolute_rotation_motion = 0.0
                 absolute_translation_motion = 0.0
@@ -1305,11 +1588,23 @@ class SquareTagGroupPoseTracker:
                     + relative_translation_motion
                     / self.relative_translation_scale_m
                     + self.reprojection_weight * reprojection_penalty
+                    + self._return_reference_cost(
+                        combination, return_reference
+                    )
                 )
-                scored.append((float(score), combination))
+                base_scored.append((float(score), combination))
 
-            if not scored:
+            if not base_scored:
                 return None, "group_pose_jump"
+            coupling_costs = self._informative_coupled_rotation_costs(
+                [combination for _, combination in base_scored]
+            )
+            scored = [
+                (base_score + coupling_cost, combination)
+                for (base_score, combination), coupling_cost in zip(
+                    base_scored, coupling_costs
+                )
+            ]
             selected = min(scored, key=lambda item: item[0])[1]
 
         aligned: dict[str, SquareTagPose] = {}
@@ -1343,4 +1638,39 @@ class SquareTagGroupPoseTracker:
 
         self._previous = aligned
         self._previous_stamp_ns = stamp
+        if (
+            direction == "decreasing"
+            and trajectory_command_u8 is not None
+            and not self._coupled_reference_rotations
+        ):
+            for (
+                driver_parent,
+                driver_child,
+                follower_parent,
+                follower_child,
+                _multiplier,
+            ) in self.coupled_rotation_pairs:
+                for pair in (
+                    (driver_parent, driver_child),
+                    (follower_parent, follower_child),
+                ):
+                    self._coupled_reference_rotations[pair] = _relative_pose(
+                        aligned[pair[0]], aligned[pair[1]]
+                    )[0]
+        if direction == "decreasing" and trajectory_command_u8 is not None:
+            self._decreasing_relative_rotations[
+                int(trajectory_command_u8)
+            ] = {
+                pair: _relative_pose(
+                    aligned[pair[0]], aligned[pair[1]]
+                )[0]
+                for pair in self.adjacent_pairs
+            }
+            if not self._task_reference_relative_poses:
+                self._task_reference_relative_poses = {
+                    pair: _relative_pose(
+                        aligned[pair[0]], aligned[pair[1]]
+                    )
+                    for pair in self.adjacent_pairs
+                }
         return dict(aligned), ""

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py

@@ -0,0 +1,237 @@
+"""Immutable G20 right-hand product identity and one-command preflight."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import hashlib
+from pathlib import Path
+import re
+from typing import Any, Mapping
+
+import yaml
+
+from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
+
+
+VIEWS = ("front", "side", "top")
+
+
+def sha256_file(path: str | Path) -> str:
+    digest = hashlib.sha256()
+    with Path(path).open("rb") as stream:
+        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _mapping(value: Any, name: str) -> Mapping[str, Any]:
+    if not isinstance(value, Mapping):
+        raise ValueError(f"{name} must be a mapping")
+    return value
+
+
+def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
+    text = str(value).strip()
+    if not text:
+        raise ValueError(f"{name} is required")
+    candidate = Path(text).expanduser()
+    if not candidate.is_absolute():
+        candidate = workspace / candidate
+    return candidate.resolve()
+
+
+def _camera_info_fingerprint(path: Path) -> str:
+    with path.open("r", encoding="utf-8") as stream:
+        payload = _mapping(yaml.safe_load(stream), str(path))
+
+    def values(key: str) -> list[float]:
+        item = _mapping(payload.get(key, {}), key)
+        return [float(value) for value in item.get("data", [])]
+
+    return camera_info_fingerprint(
+        width=int(payload["image_width"]),
+        height=int(payload["image_height"]),
+        camera_matrix=values("camera_matrix"),
+        distortion=values("distortion_coefficients"),
+        rectification=values("rectification_matrix"),
+        projection=values("projection_matrix"),
+    )
+
+
+def _tag_ids(path: Path) -> set[int]:
+    with path.open("r", encoding="utf-8") as stream:
+        payload = _mapping(yaml.safe_load(stream), str(path))
+    result: set[int] = set()
+    for node in payload.values():
+        parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters")
+        tag = _mapping(parameters.get("tag"), "tag")
+        ids = [int(value) for value in tag.get("ids", [])]
+        if result.intersection(ids):
+            raise ValueError("Tag IDs must be unique across the three views")
+        result.update(ids)
+    return result
+
+
+@dataclass(frozen=True)
+class ProductConfig:
+    path: Path
+    workspace: Path
+    serial_number: str
+    can_interface: str
+    source_urdf: Path
+    source_urdf_sha256: str
+    camera_extrinsics: Path
+    camera_extrinsics_sha256: str
+    calibration_config: Path
+    calibration_config_sha256: str
+    tag_config: Path
+    tag_config_sha256: str
+    output_root: Path
+    cameras: Mapping[str, Mapping[str, str]]
+    required_independent_passes: int
+    static_repeatability_rad: float
+
+    @property
+    def session_root(self) -> Path:
+        return self.output_root / self.serial_number
+
+
+def load_product_config(
+    path: str | Path,
+    *,
+    workspace: str | Path | None = None,
+    check_can: bool = True,
+) -> ProductConfig:
+    """Load and fully verify the fixed G20_RIGHT_001 product configuration."""
+    source = Path(path).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"product config does not exist: {source}")
+    with source.open("r", encoding="utf-8") as stream:
+        raw = _mapping(yaml.safe_load(stream), str(source))
+    if int(raw.get("schema_version", -1)) != 1:
+        raise ValueError("product config schema_version must be 1")
+    root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
+    serial = str(raw.get("serial_number", ""))
+    if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
+        raise ValueError("serial_number is invalid")
+    if raw.get("model") != "G20" or raw.get("side") != "right":
+        raise ValueError("product config must describe the G20 right hand")
+    can_interface = str(raw.get("can_interface", "")).strip()
+    if not can_interface:
+        raise ValueError("can_interface is required")
+    if check_can and not (Path("/sys/class/net") / can_interface).exists():
+        raise ValueError(f"CAN interface does not exist: {can_interface}")
+
+    artifacts = _mapping(raw.get("artifacts"), "artifacts")
+    source_urdf = _resolve_path(
+        artifacts.get("source_urdf"), workspace=root, name="source_urdf"
+    )
+    extrinsics = _resolve_path(
+        artifacts.get("camera_extrinsics"),
+        workspace=root,
+        name="camera_extrinsics",
+    )
+    calibration_config = _resolve_path(
+        artifacts.get("calibration_config"),
+        workspace=root,
+        name="calibration_config",
+    )
+    tag_config = _resolve_path(
+        artifacts.get("tag_config"), workspace=root, name="tag_config"
+    )
+    for name, candidate in (
+        ("source URDF", source_urdf),
+        ("camera extrinsics", extrinsics),
+        ("calibration config", calibration_config),
+        ("Tag config", tag_config),
+    ):
+        if not candidate.is_file():
+            raise ValueError(f"{name} does not exist: {candidate}")
+
+    expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower()
+    expected_extrinsics_hash = str(
+        artifacts.get("camera_extrinsics_sha256", "")
+    ).lower()
+    expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower()
+    expected_calibration_hash = str(
+        artifacts.get("calibration_config_sha256", "")
+    ).lower()
+    hashes = (
+        ("source URDF", source_urdf, expected_source_hash),
+        ("camera extrinsics", extrinsics, expected_extrinsics_hash),
+        ("Tag config", tag_config, expected_tag_hash),
+        ("calibration config", calibration_config, expected_calibration_hash),
+    )
+    for name, candidate, expected in hashes:
+        if re.fullmatch(r"[0-9a-f]{64}", expected) is None:
+            raise ValueError(f"{name} expected SHA-256 is invalid")
+        actual = sha256_file(candidate)
+        if actual != expected:
+            raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
+
+    expected_tag_ids = set(range(19)) - {7, 14, 16, 18}
+    if _tag_ids(tag_config) != expected_tag_ids:
+        raise ValueError(
+            "g20_right_15 Tag config must contain exactly IDs "
+            "0,1,2,3,4,5,6,8,9,10,11,12,13,15,17"
+        )
+
+    camera_raw = _mapping(raw.get("cameras"), "cameras")
+    if set(camera_raw) != set(VIEWS):
+        raise ValueError("cameras must contain front/side/top")
+    loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
+    cameras: dict[str, dict[str, str]] = {}
+    serials: set[str] = set()
+    for view in VIEWS:
+        item = _mapping(camera_raw[view], f"cameras.{view}")
+        camera_serial = str(item.get("serial_number", ""))
+        info_path = _resolve_path(
+            item.get("camera_info"), workspace=root, name=f"{view}.camera_info"
+        )
+        if not info_path.is_file():
+            raise ValueError(f"{view} camera info does not exist: {info_path}")
+        if camera_serial in serials:
+            raise ValueError("the three camera serial numbers must be unique")
+        serials.add(camera_serial)
+        identity = loaded_extrinsics.cameras[view]
+        fingerprint = _camera_info_fingerprint(info_path)
+        if camera_serial != identity.serial_number:
+            raise ValueError(f"{view} camera serial does not match extrinsics")
+        if fingerprint != identity.intrinsics_sha256:
+            raise ValueError(f"{view} camera intrinsics do not match extrinsics")
+        cameras[view] = {
+            "serial_number": camera_serial,
+            "camera_info": str(info_path),
+            "camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")),
+        }
+
+    output_root = _resolve_path(
+        raw.get("output_root", "calibration_output"),
+        workspace=root,
+        name="output_root",
+    )
+    release = _mapping(raw.get("release", {}), "release")
+    passes = int(release.get("required_independent_passes", 2))
+    if passes not in {1, 2}:
+        raise ValueError("required_independent_passes must be 1 or 2")
+    repeatability_deg = float(release.get("static_repeatability_deg", 1.0))
+    if not 0.0 < repeatability_deg <= 5.0:
+        raise ValueError("static_repeatability_deg must be in (0, 5]")
+    return ProductConfig(
+        path=source,
+        workspace=root,
+        serial_number=serial,
+        can_interface=can_interface,
+        source_urdf=source_urdf,
+        source_urdf_sha256=expected_source_hash,
+        camera_extrinsics=extrinsics,
+        camera_extrinsics_sha256=expected_extrinsics_hash,
+        calibration_config=calibration_config,
+        calibration_config_sha256=expected_calibration_hash,
+        tag_config=tag_config,
+        tag_config_sha256=expected_tag_hash,
+        output_root=output_root,
+        cameras=cameras,
+        required_independent_passes=passes,
+        static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
+    )

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py

@@ -0,0 +1,569 @@
+"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
+
+from __future__ import annotations
+
+from datetime import datetime, timezone
+import copy
+import json
+import math
+import os
+from pathlib import Path
+import re
+from typing import Any, Mapping, Sequence
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from .full_hand import (
+    G20_COMBINATION_REQUIRED_TARGET_KEYS,
+    G20_RIGHT_19_LAYOUT,
+    MIMIC_DERIVED_FINGER_DIPS,
+    get_hand_calibration_profile,
+    validate_compact_payload,
+)
+from .product import ProductConfig, sha256_file
+from .storage import atomic_write_json
+from .urdf_zero import get_zero_calibration_profile
+
+
+PASSIVE_JOINTS = frozenset(
+    {"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
+)
+ACTIVE_ZERO_JOINTS = frozenset(
+    get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
+)
+RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
+    get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
+) - ACTIVE_ZERO_JOINTS
+
+
+def _load_json(path: Path) -> dict[str, Any]:
+    with path.open("r", encoding="utf-8") as stream:
+        payload = json.load(stream)
+    if not isinstance(payload, dict):
+        raise ValueError(f"{path} must contain a JSON object")
+    return payload
+
+
+def _joint_blocks(text: str) -> dict[str, str]:
+    pattern = re.compile(
+        r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
+        re.DOTALL,
+    )
+    return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
+
+
+def _mask_origin_rpy(block: str) -> str:
+    return re.sub(
+        r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
+        r"\1\2__CALIBRATED_RPY__\2",
+        block,
+        count=1,
+    )
+
+
+def _mask_active_origin_rpy_fields(text: str) -> str:
+    pattern = re.compile(
+        r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
+        re.DOTALL,
+    )
+
+    def replace(match: re.Match[str]) -> str:
+        block = match.group(0)
+        if match.group("name") not in ACTIVE_ZERO_JOINTS:
+            return block
+        masked = _mask_origin_rpy(block)
+        if masked == block:
+            raise ValueError(
+                f"active joint {match.group('name')} has no origin.rpy"
+            )
+        return masked
+
+    return pattern.sub(replace, text)
+
+
+def _triplet(value: str) -> np.ndarray:
+    result = np.asarray([float(item) for item in value.split()], dtype=float)
+    if result.shape != (3,) or not np.all(np.isfinite(result)):
+        raise ValueError(f"invalid URDF triplet: {value}")
+    return result
+
+
+def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
+    return {
+        str(joint.get("name")): joint
+        for joint in ET.parse(Path(path)).getroot().findall("joint")
+    }
+
+
+def _verify_expected_origin_offsets(
+    source: str | Path,
+    corrected: str | Path,
+    expected_offsets_rad: Mapping[str, float],
+) -> None:
+    offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
+    if set(offsets) != ACTIVE_ZERO_JOINTS or any(
+        not math.isfinite(value) for value in offsets.values()
+    ):
+        raise ValueError("expected offsets must contain 12 finite static-zero values")
+    before = _joint_elements(source)
+    after = _joint_elements(corrected)
+    maximum_rotation_error = 0.0
+    for name, original_joint in before.items():
+        corrected_joint = after[name]
+        original_origin = original_joint.find("origin")
+        corrected_origin = corrected_joint.find("origin")
+        if original_origin is None or corrected_origin is None:
+            if original_origin is not corrected_origin:
+                raise ValueError(f"corrected URDF changed origin presence for {name}")
+            continue
+        original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
+        corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
+        if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
+            raise ValueError(f"corrected URDF changed origin.xyz for {name}")
+        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_rotation = 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")
+            )
+            norm = float(np.linalg.norm(axis))
+            if norm <= 1.0e-12:
+                raise ValueError(f"source URDF joint {name} has a degenerate axis")
+            expected_rotation = expected_rotation * Rotation.from_rotvec(
+                axis / norm * offsets[name]
+            )
+        error = float((expected_rotation.inv() * corrected_rotation).magnitude())
+        maximum_rotation_error = max(maximum_rotation_error, error)
+    # Schema v4 intentionally stores zero offsets at eight decimal places.
+    # Older sessions wrote the corrected URDF from the full-precision solve,
+    # so comparing that file with the published JSON necessarily permits half
+    # of one last-place unit.  This is about 2.9e-7 degrees and is far below
+    # any calibration or URDF numerical significance.
+    if maximum_rotation_error > 5.1e-9:
+        raise ValueError(
+            "corrected URDF origin.rpy does not match the published zero offsets"
+        )
+
+
+def verify_corrected_urdf(
+    source: str | Path,
+    corrected: str | Path,
+    *,
+    expected_offsets_rad: Mapping[str, float] | None = None,
+) -> tuple[str, ...]:
+    """Prove that only active-joint origin.rpy attributes changed.
+
+    Passive joint blocks, including their mimic elements, are compared as raw
+    UTF-8 text so formatting and numeric spelling are protected as well.
+    """
+    source_text = Path(source).read_text(encoding="utf-8")
+    corrected_text = Path(corrected).read_text(encoding="utf-8")
+    before = _joint_blocks(source_text)
+    after = _joint_blocks(corrected_text)
+    if set(before) != set(after):
+        raise ValueError("corrected URDF changed the joint set")
+    # Text patchers may normalize only the final newline.  It has no URDF/XML
+    # semantics; every joint block and every non-rpy byte remains guarded
+    # below.
+    if _mask_active_origin_rpy_fields(source_text).rstrip("\r\n") != (
+        _mask_active_origin_rpy_fields(corrected_text).rstrip("\r\n")
+    ):
+        raise ValueError("corrected URDF changed content outside active origin.rpy")
+    changed: list[str] = []
+    for name in before:
+        if before[name] == after[name]:
+            continue
+        if name not in ACTIVE_ZERO_JOINTS:
+            raise ValueError(f"corrected URDF changed protected joint {name}")
+        if _mask_origin_rpy(before[name]) != _mask_origin_rpy(after[name]):
+            raise ValueError(f"corrected URDF changed more than origin.rpy for {name}")
+        changed.append(name)
+    for name in PASSIVE_JOINTS:
+        if before.get(name) != after.get(name):
+            raise ValueError(f"passive joint/mimic block changed: {name}")
+    if expected_offsets_rad is not None:
+        _verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
+    return tuple(sorted(changed))
+
+
+def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
+    """Return every session-local mesh after proving it resolves safely."""
+    path = Path(urdf).expanduser().resolve()
+    root = ET.parse(path).getroot()
+    resources: dict[str, Path] = {}
+    for mesh in root.findall(".//mesh"):
+        filename = str(mesh.get("filename", "")).strip()
+        if not filename or "://" in filename or filename.startswith("package:"):
+            continue
+        relative = Path(filename)
+        if relative.is_absolute() or ".." in relative.parts:
+            raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
+        resolved = (path.parent / relative).resolve()
+        try:
+            resolved.relative_to(path.parent)
+        except ValueError as error:
+            raise ValueError(f"URDF mesh escapes the session: {filename}") from error
+        if not resolved.is_file():
+            raise ValueError(f"URDF mesh resource is missing: {filename}")
+        resources[relative.as_posix()] = resolved
+    return dict(sorted(resources.items()))
+
+
+def validate_runtime_curves_against_urdf_limits(
+    payload: Mapping[str, Any], source_urdf: str | Path
+) -> None:
+    """Reject a runtime curve whose commanded q leaves a CAD safety limit."""
+    validate_compact_payload(payload)
+    source_joints = _joint_elements(source_urdf)
+    for name, calibration in payload["joints"].items():
+        joint = source_joints.get(str(name))
+        if joint is None:
+            raise ValueError(f"source URDF is missing runtime joint {name}")
+        limit = joint.find("limit")
+        if limit is None or limit.get("lower") is None or limit.get("upper") is None:
+            raise ValueError(f"source URDF joint {name} has no finite position limit")
+        lower = float(limit.get("lower"))
+        upper = float(limit.get("upper"))
+        if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
+            raise ValueError(f"source URDF joint {name} has invalid position limits")
+        curve = np.asarray(calibration["angle_rad"], dtype=float)
+        minimum = float(np.min(curve))
+        maximum = float(np.max(curve))
+        tolerance = 1.0e-7
+        if minimum < lower - tolerance or maximum > upper + tolerance:
+            raise ValueError(
+                f"runtime curve exceeds source URDF limit for {name}: "
+                f"[{minimum:.9g}, {maximum:.9g}] not within "
+                f"[{lower:.9g}, {upper:.9g}]"
+            )
+
+
+def clamp_compact_payload_to_urdf_limits(
+    payload: Mapping[str, Any], source_urdf: str | Path
+) -> tuple[dict[str, Any], dict[str, int]]:
+    """Return a schema-preserving runtime payload bounded by CAD limits."""
+    result = copy.deepcopy(dict(payload))
+    validate_compact_payload(result)
+    source_joints = _joint_elements(source_urdf)
+    clipped_by_joint: dict[str, int] = {}
+    for name, calibration in result["joints"].items():
+        joint = source_joints.get(str(name))
+        limit = None if joint is None else joint.find("limit")
+        if (
+            limit is None
+            or limit.get("lower") is None
+            or limit.get("upper") is None
+        ):
+            raise ValueError(f"source URDF joint {name} has no finite position limit")
+        lower = float(limit.get("lower"))
+        upper = float(limit.get("upper"))
+        if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
+            raise ValueError(f"source URDF joint {name} has invalid position limits")
+        source = np.asarray(calibration["angle_rad"], dtype=float)
+        bounded = np.clip(source, lower, upper)
+        count = int(np.count_nonzero(bounded != source))
+        if count:
+            clipped_by_joint[str(name)] = count
+            calibration["angle_rad"] = [
+                round(float(value), 8) for value in bounded
+            ]
+    validate_compact_payload(result)
+    return result, clipped_by_joint
+
+
+def build_mujoco_validation_commands(
+    baseline: Sequence[int],
+) -> dict[str, Any]:
+    if len(baseline) != 20:
+        raise ValueError("baseline must contain exactly 20 commands")
+    base = [int(value) for value in baseline]
+    if any(not 0 <= value <= 255 for value in base):
+        raise ValueError("baseline commands must be in [0, 255]")
+
+    def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
+        values = list(base)
+        for index, value in changes.items():
+            values[int(index)] = int(value)
+        return {"name": name, "label_zh": label, "command_u8": values}
+
+    return {
+        "schema_version": 1,
+        "topic": "/g20/cb_right_hand_control_cmd",
+        "serial_number": "G20_RIGHT_001",
+        "poses": [
+            pose("all_open", "全开", {}),
+            pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
+            pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
+            pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
+            pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
+            pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
+            pose(
+                "half_grip",
+                "四指半握",
+                {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
+            ),
+            pose(
+                "light_pinch",
+                "轻捏",
+                {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
+            ),
+        ],
+    }
+
+
+def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
+    parent = Path(root).resolve()
+    target = Path(session).resolve()
+    if target.parent != parent:
+        raise ValueError("session pointer target must be a direct child of the serial root")
+    if not target.is_dir():
+        raise ValueError(f"session directory does not exist: {target}")
+    if name not in {"latest_attempt", "latest_passed"}:
+        raise ValueError("unsupported session pointer name")
+    parent.mkdir(parents=True, exist_ok=True)
+    destination = parent / name
+    temporary = parent / f".{name}.{os.getpid()}.tmp"
+    if temporary.is_symlink():
+        temporary.unlink()
+    elif temporary.exists():
+        raise ValueError(f"temporary pointer path is occupied: {temporary}")
+    os.symlink(target.name, temporary, target_is_directory=True)
+    os.replace(temporary, destination)
+    return destination
+
+
+def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
+    validate_compact_payload(payload)
+    result = {
+        name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
+        for name in ACTIVE_ZERO_JOINTS
+    }
+    if set(result) != ACTIVE_ZERO_JOINTS or any(
+        not math.isfinite(value) for value in result.values()
+    ):
+        raise ValueError("payload does not contain 12 finite observable zero offsets")
+    return result
+
+
+def compare_session_offsets(
+    first: Mapping[str, Any],
+    second: Mapping[str, Any],
+    *,
+    maximum_difference_rad: float,
+) -> dict[str, float]:
+    left = active_offsets(first)
+    right = active_offsets(second)
+    differences = {name: abs(left[name] - right[name]) for name in sorted(left)}
+    failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
+    if failed:
+        details = ", ".join(
+            f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
+        )
+        raise ValueError(f"independent-session static-zero mismatch: {details}")
+    return differences
+
+
+def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
+    directory = Path(session).resolve()
+    urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
+    if len(urdfs) != 1:
+        raise ValueError("session must contain exactly one corrected URDF")
+    return {
+        "json": directory / f"g20_right_{serial_number}_calibration.json",
+        "urdf": urdfs[0],
+        "summary": directory / "calibration_summary_zh.json",
+        "commands": directory / "mujoco_validation_commands.json",
+        "raw": directory / "raw_samples.jsonl",
+        "log": directory / "calibration.log",
+    }
+
+
+def find_compatible_prior_session(
+    config: ProductConfig,
+    current: str | Path,
+    current_payload: Mapping[str, Any],
+) -> tuple[Path | None, dict[str, float]]:
+    current_path = Path(current).resolve()
+    for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
+        if candidate.resolve() == current_path:
+            continue
+        json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
+        summary_path = candidate / "calibration_summary_zh.json"
+        if not json_path.is_file() or not summary_path.is_file():
+            continue
+        try:
+            summary = _load_json(summary_path)
+            if not bool(summary.get("quality", {}).get("passed")):
+                continue
+            payload = _load_json(json_path)
+            differences = compare_session_offsets(
+                payload,
+                current_payload,
+                maximum_difference_rad=config.static_repeatability_rad,
+            )
+        except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
+            continue
+        return candidate, differences
+    return None, {}
+
+
+def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
+    # The formal product uses the independently held-out fourth sweep cycle.
+    # The optional eight-pose check is a developer diagnostic because the
+    # 15-Tag layout has no DIP Tags and the axis-line zero solve does not
+    # establish an absolute Cartesian hand-base transform.  When explicitly
+    # enabled, retain its strict coverage and error checks.
+    if not bool(combination.get("enabled")):
+        return
+    position_p95 = float(combination.get("position_p95_m", float("inf")))
+    orientation_p95 = float(
+        combination.get("orientation_p95_rad", float("inf"))
+    )
+    required = {str(value) for value in combination.get("required_targets", ())}
+    expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
+    observations = combination.get("observation_counts")
+    validations = combination.get("validation_counts")
+    if required != expected:
+        raise ValueError("combination validation has the wrong required-target set")
+    if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
+        raise ValueError("combination validation is missing per-target coverage")
+    missing_observations = sorted(
+        key for key in expected if int(observations.get(key, 0)) < 2
+    )
+    missing_validations = sorted(
+        key for key in expected if int(validations.get(key, 0)) < 1
+    )
+    if missing_observations or missing_validations:
+        raise ValueError(
+            "combination validation target coverage is incomplete: "
+            f"observations={','.join(missing_observations) or '-'}; "
+            f"validations={','.join(missing_validations) or '-'}"
+        )
+    if (
+        not bool(combination.get("completed"))
+        or int(combination.get("completed_poses", 0)) != 8
+        or position_p95 > 0.003
+        or orientation_p95 > math.radians(2.0)
+    ):
+        raise ValueError(
+            "eight-pose four-finger combination validation is incomplete or failed"
+        )
+
+
+def finalize_session_artifacts(
+    config: ProductConfig,
+    session: str | Path,
+    *,
+    node_status: Mapping[str, Any],
+) -> tuple[dict[str, Any], bool]:
+    """Create diagnostics and update latest_passed only after every guard."""
+    directory = Path(session).resolve()
+    paths = session_artifact_paths(directory, config.serial_number)
+    for name in ("json", "urdf", "raw", "log"):
+        if not paths[name].is_file():
+            raise ValueError(f"session is missing {name}: {paths[name]}")
+    payload = _load_json(paths["json"])
+    validate_compact_payload(payload)
+    if payload.get("schema_version") != 4 or payload.get("side") != "right":
+        raise ValueError("runtime JSON is not the compact right-hand schema v4")
+    if not bool(payload.get("quality", {}).get("passed")):
+        raise ValueError("runtime JSON quality is not passed")
+    payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
+        payload, config.source_urdf
+    )
+    if clipped_runtime_joints:
+        # A node from before runtime-limit saturation may already have written
+        # an otherwise valid commit candidate.  Normalize it before hashing
+        # and publication so completed sessions can be recovered offline.
+        atomic_write_json(paths["json"], payload)
+    combination = node_status.get("combination_validation")
+    if not isinstance(combination, Mapping):
+        raise ValueError("node status is missing combination validation")
+    _verify_combination_validation(combination)
+    offsets = active_offsets(payload)
+    changed_joints = verify_corrected_urdf(
+        config.source_urdf,
+        paths["urdf"],
+        expected_offsets_rad=offsets,
+    )
+    validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
+    mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
+    mesh_hashes = {
+        name: sha256_file(path) for name, path in mesh_resources.items()
+    }
+    commands = build_mujoco_validation_commands(payload["baseline_command_u8"])
+    atomic_write_json(paths["commands"], commands)
+
+    prior, differences = find_compatible_prior_session(config, directory, payload)
+    release_ready = config.required_independent_passes == 1 or prior is not None
+    quality = dict(payload["quality"])
+    summary: dict[str, Any] = {
+        "schema_version": 1,
+        "serial_number": config.serial_number,
+        "session_id": f"{config.serial_number}_{directory.name}",
+        "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
+        "quality": quality,
+        "runtime_limit_clipped_bins": clipped_runtime_joints,
+        "runtime_curve_domain": "requested_command_u8",
+        "static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS),
+        "retained_active_urdf_zero_joints": sorted(
+            RETAINED_ACTIVE_ZERO_JOINTS
+        ),
+        "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
+        "mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS),
+        "changed_urdf_joint_origins": list(changed_joints),
+        "hashes": {
+            "source_urdf_sha256": config.source_urdf_sha256,
+            "camera_extrinsics_sha256": config.camera_extrinsics_sha256,
+            "calibration_config_sha256": config.calibration_config_sha256,
+            "corrected_urdf_sha256": sha256_file(paths["urdf"]),
+            "mesh_resources_sha256": mesh_hashes,
+            "calibration_json_sha256": sha256_file(paths["json"]),
+            "raw_samples_sha256": sha256_file(paths["raw"]),
+        },
+        "holdout": {
+            "training_cycles": [0, 1, 2],
+            "validation_cycle": 3,
+            "isolated": True,
+        },
+        "combination_validation": dict(combination),
+        "formal_release": {
+            "passed": release_ready,
+            "required_independent_passes": config.required_independent_passes,
+            "comparison_session": None if prior is None else prior.name,
+            "maximum_static_difference_deg": (
+                None if not differences else math.degrees(max(differences.values()))
+            ),
+        },
+        "node_status": dict(node_status),
+        "created_at_utc": datetime.now(timezone.utc).isoformat(),
+        "artifacts": {name: path.name for name, path in paths.items()},
+    }
+    atomic_write_json(paths["summary"], summary)
+    # Recompute hashes after all files exist and validate the pair once more
+    # immediately before the one atomic publication operation.
+    if sha256_file(config.source_urdf) != config.source_urdf_sha256:
+        raise ValueError("source URDF changed during calibration")
+    verify_corrected_urdf(
+        config.source_urdf,
+        paths["urdf"],
+        expected_offsets_rad=offsets,
+    )
+    validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
+    final_mesh_hashes = {
+        name: sha256_file(path)
+        for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
+    }
+    if final_mesh_hashes != mesh_hashes:
+        raise ValueError("URDF mesh resources changed during publication")
+    if release_ready:
+        atomic_session_pointer(config.session_root, "latest_passed", directory)
+    return summary, release_ready

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py

@@ -48,6 +48,10 @@ JOINT_NAMES_ZH = {
     "pinky_pip": "小指PIP",
     "pinky_dip": "小指DIP（被动）",
     "thumb_cmc_yaw": "拇指CMC侧摆",
+    "index_mcp_roll_side": "食指MCP侧摆（侧面校验）",
+    "middle_mcp_roll_side": "中指MCP侧摆（侧面校验）",
+    "ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）",
+    "pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）",
 }
 
 
@@ -86,6 +90,12 @@ def _task_text(active: Mapping[str, Any]) -> str:
             f"{_format_u8(active.get('target_u8'))}、实际"
             f"{_format_u8(active.get('actual_u8'))}"
         )
+    if active.get("kind") == "cross_view_roll_diagnostic":
+        return (
+            f"{active.get('finger', '?')}侧摆跨机位诊断完成："
+            f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，"
+            f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
+        )
     if active.get("kind") == "validation":
         return (
             f"{view}机位，随机复测，电机{active.get('motor_index')}，"
@@ -132,10 +142,19 @@ def three_camera_reason_zh(
         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 "未知"
+        timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
+        timeout_value = active.get("timeout_seconds")
+        if timeout_value is None and timeout_match is not None:
+            timeout_value = float(timeout_match.group(1))
+        duration = (
+            f"连续{float(timeout_value):g}秒"
+            if timeout_value is not None
+            else "在规定时间内"
+        )
         motor = active.get("motor_index")
         if motor is not None:
             return (
-                f"电机{motor}反馈连续8秒没有向目标推进；目标"
+                f"电机{motor}反馈{duration}没有向目标推进；目标"
                 f"{_format_u8(active.get('target_u8'))}、实际"
                 f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
                 f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
@@ -144,12 +163,29 @@ def three_camera_reason_zh(
                 "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
             )
         return (
-            f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
+            f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8"
             f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
             "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
             "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
         )
 
+    base_reason, separator, reason_detail = reason.partition(":")
+    if base_reason in {
+        "sweep_missing_endpoint_bin",
+        "sweep_bins_too_few",
+        "sweep_bin_gap_too_large",
+        "task_precheck_missing_command_127",
+        "task_precheck_detection_rate_too_low",
+        "synchronised_tag_state_timeout",
+    }:
+        reason = base_reason
+    detail_label = (
+        JOINT_NAMES_ZH.get(reason_detail, reason_detail)
+        if separator and reason_detail
+        else ""
+    )
+    detail_prefix = f"{detail_label}：" if detail_label else ""
+
     if "URDF zero offset reached the configured" in reason:
         bound_match = re.search(
             r"configured\s+([0-9.]+)\s+degree bound", reason
@@ -174,7 +210,7 @@ def three_camera_reason_zh(
     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"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
             f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
             f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
             "端点，但该时刻没有同时取得有效Tag图像和电机状态。",
@@ -183,29 +219,64 @@ def three_camera_reason_zh(
         )
     if reason == "sweep_bins_too_few":
         return (
-            f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
+            f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
             f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
             "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
         )
     if reason == "sweep_bin_gap_too_large":
+        gap_start = sample.get("maximum_bin_gap_start_u8")
+        gap_end = sample.get("maximum_bin_gap_end_u8")
+        gap_range = (
+            ""
+            if gap_start is None or gap_end is None
+            else f"（{gap_start}→{gap_end}）"
+        )
         return (
-            f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
+            f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
-            f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
+            f"{gap_range}允许值不超过"
+            f"{sample.get('allowed_maximum_bin_gap', '?')}。",
             "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
         )
     if reason == "synchronised_tag_state_timeout":
+        group_reasons = active.get("group_pnp_reasons", {})
+        if isinstance(group_reasons, Mapping) and group_reasons:
+            reason_text = "、".join(
+                f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
+                for view, value in group_reasons.items()
+            )
+            return (
+                f"{detail_prefix}Tag仍可见且反馈正常，但连续图像帧被整组PnP几何检查拒绝"
+                f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。",
+                "不要调整或反复粘贴Tag；保留当前会话并复制诊断块给开发者检查PnP分支逻辑。",
+            )
         return (
-            "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
+            f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
             "按时间戳配对”的图像帧。",
             "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
             "程序会重扫当前方向。",
         )
+    if reason == "task_precheck_missing_command_127":
+        return (
+            f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
+            "检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。",
+        )
+    if reason == "task_precheck_detection_rate_too_low":
+        return (
+            f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
+            "检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。",
+        )
     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_start_tag_timeout":
+        return (
+            "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的"
+            "同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。",
+            "只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。",
+        )
     if reason == "sweep_timeout":
         return (
             "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
@@ -226,7 +297,7 @@ def three_camera_reason_zh(
             "随机复测位置没有采集到足够的同步有效Tag帧。",
             "检查当前机位Tag可见性后调用resume。",
         )
-    if reason == "joint_fit_check_failed":
+    if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
         metric_names = {
             "plane_rms_mm": "平面拟合RMS",
             "radial_rms_mm": "圆半径拟合RMS",
@@ -237,6 +308,9 @@ def three_camera_reason_zh(
             "arc_deg": "实测圆弧",
             "monotonic_correction_deg": "最大单调修正",
             "hysteresis_deg": "最大正反程差",
+            "baseline_hysteresis_deg": "baseline正反程关节角差",
+            "baseline_directional_gap_deg": "baseline方向分支间隙",
+            "baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
             "cycle_travel_range_deg": "三轮行程差",
             "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
             "axis_plane_rms_mm": "三维圆轴向RMS",
@@ -259,6 +333,9 @@ def three_camera_reason_zh(
             "arc_deg": "°",
             "monotonic_correction_deg": "°",
             "hysteresis_deg": "°",
+            "baseline_hysteresis_deg": "°",
+            "baseline_directional_gap_deg": "°",
+            "baseline_directional_gap_range_deg": "°",
             "cycle_travel_range_deg": "°",
             "rotation_orthogonal_rms_deg": "°",
             "axis_plane_rms_mm": "mm",
@@ -288,9 +365,14 @@ def three_camera_reason_zh(
                 )
                 cycle_travel = failure.get("cycle_travel_deg", [])
                 if cycle_travel:
-                    detail += "（三轮=" + "/".join(
+                    detail += "（各轮=" + "/".join(
                         f"{float(value):.2f}°" for value in cycle_travel
                     ) + "）"
+                cycle_values = failure.get("cycle_values_deg", [])
+                if cycle_values:
+                    detail += "（各轮=" + "/".join(
+                        f"{float(value):.2f}°" for value in cycle_values
+                    ) + "）"
                 details.append(detail)
             else:
                 cycle = failure.get("cycle")
@@ -300,11 +382,30 @@ def three_camera_reason_zh(
                     f"{failure.get('reason', '未知原因')}"
                 )
         detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
+        directional_gap_failure = any(
+            str(failure.get("metric", "")).startswith(
+                "baseline_directional_gap"
+            )
+            for failure in active.get("failures", [])
+        )
+        if reason == "joint_fit_systematic_failure":
+            suggestion = (
+                "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。"
+                "请直接复制诊断块给开发者，不要放宽门限。"
+            )
+        else:
+            suggestion = (
+                "方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，"
+                "处理后重新执行一键标定命令，程序会从最近可靠断点继续。"
+                if directional_gap_failure
+                else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；"
+                "程序只清除当前失败关节的数据并重扫"
+                f"{active.get('directions_to_rescan', 6)}个方向，"
+                "不需要手工调用ROS服务。"
+            )
         return (
             detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
-            "修正Tag位置、遮挡或机械行程后调用"
+            suggestion,
-            "/g20_calibration/resume；程序只清除当前失败关节的数据"
-            f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
         )
     if reason == "zero_model_validation_failed":
         reason_names = {
@@ -345,11 +446,37 @@ def three_camera_reason_zh(
             "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
             "请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
         )
+    if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
+        return (
+            "正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。",
+            "保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。",
+        )
     if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
         return (
             "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
             "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
         )
+    if reason == "waiting_for_baseline_tags_after_recovery":
+        return (
+            "机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。",
+            "若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。",
+        )
+    if reason == "locking_fixed_base_references":
+        return (
+            "基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
+            "无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。",
+        )
+    if reason == "waiting_for_task_tags_at_sweep_start":
+        return (
+            "电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag"
+            "允许被手指遮挡。",
+            "只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。",
+        )
+    if reason == "call_start_for_baseline_recovery":
+        return (
+            "相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。",
+            "保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。",
+        )
     if reason == "call_start":
         return (
             "三机位预检已经通过，等待操作员确认开始。",
@@ -361,14 +488,62 @@ def three_camera_reason_zh(
         return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
     if reason == "collecting_timestamp_synchronised_tag_centres":
         return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
+    if reason == "collecting_dedicated_baseline_hold":
+        return (
+            "正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。",
+            "无需操作，保持相机、标签和底座不动。",
+        )
+    if reason == "steady checkpoint target is missing":
+        return (
+            "首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。",
+            "程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。",
+        )
+    if reason == "cross_view_roll_front_failure_deferred":
+        return (
+            "正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。",
+            "无需操作；该诊断会锁定URDF发布。",
+        )
+    if reason == "cross_view_roll_diagnostic_complete":
+        interpretation = str(active.get("interpretation", ""))
+        explanations = {
+            "both_views_confirm_direction_dependent_pose": (
+                "正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。"
+            ),
+            "front_only_difference_check_roll_tag_bracket_or_front_pnp": (
+                "只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。"
+            ),
+            "side_only_difference_check_side_tag_chain_or_side_pnp": (
+                "只有侧面差异超限，优先检查侧面Tag链和侧面PnP。"
+            ),
+            "both_views_within_formal_hysteresis_limit": (
+                "两个机位的静止回差均满足正式门限。"
+            ),
+        }
+        return (
+            explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
+            + " 本次为诊断会话，不会生成或发布URDF。",
+            "保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。",
+        )
     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 == "combination_pose_prediction_failed":
+        return (
+            "单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
+            "程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的"
+            "combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。",
+        )
     if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
         return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
+    if reason == "holding_same_finger_clearance_before_next_task":
+        return (
+            "同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整"
+            "被测关节以衔接下一项。",
+            "无需操作，不要手动展开正在避让的手指。",
+        )
     if state == "RETURN_BASELINE":
         return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
     if state == "FITTING":
@@ -431,6 +606,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
             f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
             f"{_format_u8(sample.get('maximum_u8'))}"
         )
+        detection_frames = int(active.get("detection_frames", 0))
+        if detection_frames:
+            lines.append(
+                "本方向Tag检出："
+                f"{float(active.get('detection_rate', 0.0)):.1%}"
+                f"（{active.get('detection_valid_frames', 0)}/"
+                f"{detection_frames}帧）"
+            )
         auxiliary = active.get("auxiliary_motors", [])
         if auxiliary:
             lines.append(

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py

@@ -634,6 +634,7 @@ def _fit_joint_curve(
     values: Sequence[float],
     *,
     endpoint_reference: Mapping[str, Sequence[float]] | None = None,
+    preserve_direction_offset: bool = False,
 ) -> tuple[dict[str, Any], float, float]:
     by_direction: dict[str, list[list[float]]] = {
         direction: [[] for _ in range(256)] for direction in DIRECTIONS
@@ -669,9 +670,11 @@ def _fit_joint_curve(
             ],
             dtype=float,
         )
-        raw -= raw[-1]
+        if not preserve_direction_offset:
+            raw -= raw[-1]
         projected_samples = isotonic_nonincreasing(raw)
-        projected_samples -= projected_samples[-1]
+        if not preserve_direction_offset:
+            projected_samples -= projected_samples[-1]
         maximum_correction = max(
             maximum_correction,
             float(np.max(np.abs(projected_samples - raw))),
@@ -681,7 +684,8 @@ def _fit_joint_curve(
             commands.astype(float),
             projected_samples,
         )
-        curve -= curve[255]
+        if not preserve_direction_offset:
+            curve -= curve[255]
         if endpoint_reference is not None:
             curve = _regularize_coupled_zero_tail(
                 curve,
@@ -695,7 +699,8 @@ def _fit_joint_curve(
     increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
     hysteresis = float(np.max(np.abs(decreasing - increasing)))
     combined = 0.5 * (decreasing + increasing)
-    combined -= combined[255]
+    if not preserve_direction_offset:
+        combined -= combined[255]
     return (
         {
             "angle_rad": [round(float(value), 8) for value in combined],

src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py

@@ -1,4 +1,4 @@
-"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
+"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument

src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py

@@ -3,6 +3,7 @@
 from __future__ import annotations
 
 from datetime import datetime
+import hashlib
 from pathlib import Path
 import re
 
@@ -41,6 +42,28 @@ def _launch_stack(context):
     hand_type = LaunchConfiguration("hand_type").perform(context).lower()
     if hand_type not in {"left", "right"}:
         raise RuntimeError("hand_type must be left or right")
+    tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
+    if tag_layout not in {"legacy_11", "g20_right_15"}:
+        raise RuntimeError("tag_layout must be legacy_11 or g20_right_15")
+    if tag_layout == "g20_right_15" and hand_type != "right":
+        raise RuntimeError("g20_right_15 requires hand_type:=right")
+    requested_tag_config = LaunchConfiguration("tag_config").perform(context)
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    tag_config = (
+        Path(requested_tag_config).expanduser().resolve()
+        if requested_tag_config
+        else package_share
+        / "config"
+        / (
+            "three_camera_tags_g20_right_15.yaml"
+            if tag_layout == "g20_right_15"
+            else "three_camera_tags.yaml"
+        )
+    )
+    if not tag_config.is_file():
+        raise RuntimeError(f"tag config does not exist: {tag_config}")
     command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
     state_topic = f"/g20/cb_{hand_type}_hand_state"
     info_topic = f"/g20/cb_{hand_type}_hand_info"
@@ -52,6 +75,20 @@ def _launch_stack(context):
     )
     if not source_urdf.is_file():
         raise RuntimeError(f"source URDF does not exist: {source_urdf}")
+    expected_source_hash = LaunchConfiguration(
+        "source_urdf_expected_sha256"
+    ).perform(context).strip().lower()
+    if tag_layout == "g20_right_15":
+        if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
+            raise RuntimeError(
+                "g20_right_15 requires source_urdf_expected_sha256 confirmed "
+                "by the CAD/hardware owner"
+            )
+        actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
+        if actual_source_hash != expected_source_hash:
+            raise RuntimeError(
+                "source_urdf_expected_sha256 does not match source_urdf_path"
+            )
 
     hand_serial = LaunchConfiguration("serial_number").perform(context)
     if (
@@ -163,7 +200,7 @@ def _launch_stack(context):
                     name="apriltag",
                     namespace=detector_namespace,
                     parameters=[
-                        LaunchConfiguration("tag_config"),
+                        str(tag_config),
                         {
                             "detector.decimate": ParameterValue(
                                 LaunchConfiguration("apriltag_decimate"),
@@ -234,7 +271,11 @@ def _launch_stack(context):
             {
                 "serial_number": hand_serial,
                 "hand_type": hand_type,
+                "tag_layout": tag_layout,
                 "session_dir": str(session_dir),
+                "resume_raw_samples_path": LaunchConfiguration(
+                    "resume_raw_samples_path"
+                ),
                 # 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
@@ -246,6 +287,9 @@ def _launch_stack(context):
                     "camera_extrinsics_file"
                 ),
                 "source_urdf_path": str(source_urdf),
+                "source_urdf_expected_sha256": LaunchConfiguration(
+                    "source_urdf_expected_sha256"
+                ),
                 "corrected_urdf_output_dir": LaunchConfiguration(
                     "corrected_urdf_output_dir"
                 ),
@@ -267,6 +311,13 @@ def _launch_stack(context):
                     LaunchConfiguration("index_flex_calibration_speed"),
                     value_type=int,
                 ),
+                "adaptive_formal_speed_enabled": ParameterValue(
+                    LaunchConfiguration("adaptive_formal_speed_enabled"),
+                    value_type=bool,
+                ),
+                "cross_view_roll_diagnostic_finger": LaunchConfiguration(
+                    "cross_view_roll_diagnostic_finger"
+                ),
                 "validation_enabled": ParameterValue(
                     LaunchConfiguration("validation_enabled"), value_type=bool
                 ),
@@ -300,7 +351,7 @@ def _launch_stack(context):
     return [
         LogInfo(
             msg=(
-                f"G20 {hand_type} three-camera session: {session_dir}; "
+                f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
                 f"source_urdf={source_urdf}"
             )
         ),
@@ -342,6 +393,7 @@ def generate_launch_description() -> LaunchDescription:
                 value=str(package_share / "config" / "fastdds_large_images.xml"),
             ),
             DeclareLaunchArgument("hand_type", default_value="left"),
+            DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
             DeclareLaunchArgument(
                 "front_camera_serial", default_value="DB2163742"
@@ -387,6 +439,12 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "index_flex_calibration_speed", default_value="10"
             ),
+            DeclareLaunchArgument(
+                "adaptive_formal_speed_enabled", default_value="true"
+            ),
+            DeclareLaunchArgument(
+                "cross_view_roll_diagnostic_finger", default_value=""
+            ),
             DeclareLaunchArgument("validation_enabled", default_value="false"),
             DeclareLaunchArgument(
                 "camera_extrinsics_file",
@@ -397,6 +455,9 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "source_urdf_path", default_value=""
             ),
+            DeclareLaunchArgument(
+                "source_urdf_expected_sha256", default_value=""
+            ),
             DeclareLaunchArgument(
                 "corrected_urdf_output_dir", default_value=""
             ),
@@ -409,6 +470,7 @@ def generate_launch_description() -> LaunchDescription:
                 default_value=str(Path.cwd() / "calibration_output"),
             ),
             DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
             DeclareLaunchArgument(
                 "calibration_config",
                 default_value=str(
@@ -417,9 +479,7 @@ def generate_launch_description() -> LaunchDescription:
             ),
             DeclareLaunchArgument(
                 "tag_config",
-                default_value=str(
+                default_value="",
-                    package_share / "config" / "three_camera_tags.yaml"
-                ),
             ),
             OpaqueFunction(function=_launch_stack),
         ]

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>Three-view Hikrobot AprilTag calibration for the complete left G20 hand.</description>
+  <description>One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right hand.</description>
   <maintainer email="support@linker-robotics.com">lxp</maintainer>
   <license>MIT</license>
 

src/g20_thumb_apriltag_calibration/setup.py

@@ -26,7 +26,7 @@ setup(
     zip_safe=True,
     maintainer="lxp",
     maintainer_email="support@linker-robotics.com",
-    description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
+    description="One-command three-camera AprilTag calibration for the G20 right hand",
     license="MIT",
     entry_points={
         "console_scripts": [
@@ -64,6 +64,10 @@ setup(
                 "g20_thumb_apriltag_calibration."
                 "calibrated_joint_state_bridge:main"
             ),
+            (
+                "calibrate_g20_right = "
+                "g20_thumb_apriltag_calibration.one_command:main"
+            ),
         ],
     },
 )

src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py

@@ -6,10 +6,16 @@ from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
     G20_COMMAND_NAMES,
     G20_URDF_JOINT_NAMES,
     CalibratedCommandMapper,
+    default_input_topic,
 )
 from g20_thumb_apriltag_calibration.full_hand import (
+    JointCurveFit,
+    build_compact_payload,
     get_hand_calibration_profile,
 )
+from g20_thumb_apriltag_calibration.urdf_zero import (
+    get_zero_calibration_profile,
+)
 
 
 def _payload(side: str = "right") -> dict:
@@ -85,3 +91,87 @@ 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_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
+    profile = get_hand_calibration_profile("right", "g20_right_15")
+    zero = get_zero_calibration_profile("right", "g20_right_15")
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    fits = {}
+    diagnostics = {}
+    for name, spec in profile.joint_specs.items():
+        zero_command = baseline[spec.motor_index]
+        average = tuple(
+            0.001 * (zero_command - value) for value in range(256)
+        )
+        decreasing = tuple(1.1 * value for value in average)
+        increasing = tuple(0.9 * value for value in average)
+        fits[name] = JointCurveFit(
+            average,
+            decreasing,
+            increasing,
+            {},
+            0.0,
+            0.0,
+            {"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
+        )
+        diagnostics[name] = {
+            "cycle_travel_rad": [0.25] * 4,
+            "cycle_travel_range_rad": 0.0,
+            "baseline_hysteresis_by_cycle_rad": [0.0] * 4,
+            "holdout_cycle": 3,
+            "holdout_cycle_mae_rad": 0.0,
+            "holdout_cycle_max_rad": 0.0,
+        }
+    payload = build_compact_payload(
+        serial_number="TEST_RIGHT_V5",
+        measured_fits=fits,
+        urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
+        validation_errors_rad=[0.0],
+        passed=True,
+        baseline=baseline,
+        side="right",
+        layout_id="g20_right_15",
+        zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
+        zero_cycle_offsets_rad={
+            name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
+        },
+        zero_observers=zero.offset_observer_joint,
+        artifact_hashes={
+            "source_urdf_sha256": "0" * 64,
+            "camera_extrinsics_sha256": "1" * 64,
+            "corrected_urdf_sha256": "2" * 64,
+        },
+        cross_view_roll_metrics={
+            f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
+            for finger in ("index", "middle", "ring", "pinky")
+        },
+        joint_dynamic_diagnostics=diagnostics,
+        zero_geometry_diagnostics={
+            "training_cycles": (0, 1, 2),
+            "validation_cycle": 3,
+            "axis_line_rms_m": 0.0,
+            "validation_line_error_by_joint_m": {},
+        },
+    )
+    mapper = CalibratedCommandMapper(payload, expected_side="right")
+    command = [255.0] * 20
+    command[0] = 100.0
+    mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
+
+    assert mapper.input_domain == "command_u8"
+    assert mapper.layout_id == "g20_right_15"
+    # midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
+    assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
+    assert payload["joints"]["pinky_dip"]["passive"] is True
+    assert "source_joint" not in payload["joints"]["pinky_dip"]
+
+
+def test_default_topics_use_the_g20_sdk_namespace() -> None:
+    assert default_input_topic("right", "command_u8") == (
+        "/g20/cb_right_hand_control_cmd"
+    )
+    assert default_input_topic("right", "feedback_u8") == (
+        "/g20/cb_right_hand_state"
+    )

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -80,6 +80,36 @@ def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
     ]["ros__parameters"]["tag"]["frames"][0] == "side_base"
 
 
+def test_right_15_tag_config_matches_the_physical_layout() -> None:
+    tags = yaml.safe_load(
+        (
+            PACKAGE_ROOT
+            / "config"
+            / "three_camera_tags_g20_right_15.yaml"
+        ).read_text()
+    )
+    expected = {
+        "front": [0, 1, 2, 3, 10, 11, 12, 13],
+        "side": [4, 5, 6, 15, 17],
+        "top": [8, 9],
+    }
+    all_ids: set[int] = set()
+    for view, ids in expected.items():
+        parameters = tags[
+            f"/g20_calibration/{view}/apriltag/apriltag"
+        ]["ros__parameters"]
+        assert parameters["tag"]["ids"] == ids
+        assert parameters["tag"]["sizes"] == [0.016] * len(ids)
+        all_ids.update(ids)
+    assert all_ids == set(range(19)) - {7, 14, 16, 18}
+    side_frames = tags[
+        "/g20_calibration/side/apriltag/apriltag"
+    ]["ros__parameters"]["tag"]["frames"]
+    assert side_frames == [
+        "side_base", "ring_pip", "pinky_pip", "middle_pip", "index_pip",
+    ]
+
+
 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()
@@ -114,14 +144,35 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
     assert parameters["normal_calibration_speed"] == 15
     assert parameters["index_roll_calibration_speed"] == 5
     assert parameters["index_flex_calibration_speed"] == 10
+    assert parameters["adaptive_formal_speed_enabled"] is True
+    assert parameters["adaptive_formal_speed_max_scale"] == 1.5
+    assert parameters["adaptive_formal_speed_minimum_bins"] == 64
+    assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
     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["thumb_ip_pnp_coupling_multiplier"] == 1.03
+    assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
+    assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
+    assert parameters["baseline_hold_seconds"] == 0.5
+    assert parameters["minimum_baseline_hold_frames"] == 10
+    assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
+    assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
+    assert (
+        parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
+        == 0.3
+    )
+    assert parameters["cross_view_roll_diagnostic_finger"] == ""
     assert parameters["repetitions"] == 3
+    assert parameters["g20_right_19_repetitions"] >= 4
     assert parameters["validation_enabled"] is False
+    assert parameters["combination_validation_enabled"] is False
+    assert parameters["combination_validation_frames"] >= 10
+    assert parameters["combination_maximum_position_p95_m"] <= 0.003
+    assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
     assert parameters["minimum_detection_rate"] == 0.95
     assert parameters["minimum_detection_hz"] == 15.0
     assert parameters["minimum_state_span_u8"] >= 240.0
@@ -130,14 +181,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
     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_sweep_retry_limit"] == 2
     assert parameters["automatic_fit_retry_limit"] == 2
-    assert parameters["motor_stall_timeout_seconds"] >= 5.0
+    assert parameters["motor_stall_timeout_seconds"] == 2.0
+    assert parameters["motor_stall_startup_grace_seconds"] == 1.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_speed_scales"] == [0.8, 0.6]
-    assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
+    assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
     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
@@ -147,6 +199,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
     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_observability_condition_number"] >= 1.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
@@ -155,7 +208,13 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
     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
+    assert parameters["maximum_hysteresis_deg"] <= 2.0
+    assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
+    assert parameters["command_maximum_direction_gap_deg"] <= 2.0
+    assert parameters["maximum_validation_mae_deg"] <= 1.0
+    assert parameters["maximum_validation_p95_deg"] <= 2.0
+    assert parameters["maximum_validation_error_deg"] <= 3.0
+    assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5
     for view in ("front", "side", "top"):
         assert parameters[f"{view}_camera_info_topic"].startswith(
             f"/g20_calibration/{view}/"

src/g20_thumb_apriltag_calibration/test/test_full_hand.py

@@ -1,5 +1,6 @@
 import math
 from dataclasses import replace
+from pathlib import Path
 
 import numpy as np
 import pytest
@@ -8,18 +9,26 @@ from g20_thumb_apriltag_calibration.full_hand import (
     ACTIVE_JOINTS,
     IMAGE_TRAJECTORY_JOINTS,
     JOINT_SPECS,
+    JointCurveFit,
     LEFT_HAND_PROFILE,
     MEASURED_JOINTS,
     PASSIVE_JOINTS,
     RIGHT_HAND_PROFILE,
+    RIGHT_19_HAND_PROFILE,
     SPLAY_JOINTS,
     SWEEP_SPECS,
+    THREE_CAMERA_BASELINE_COMMAND,
     VIEW_TAGS,
     build_calibration_motion_command,
+    build_calibration_preparation_waypoints,
+    build_calibration_return_waypoints,
     build_calibration_speed_profile,
     build_compact_payload,
     build_full_hand_command,
+    clamp_runtime_fits_to_urdf_limits,
     center_splay_curve,
+    compare_cross_view_roll_curves,
+    derive_mimic_passive_fits,
     fit_joint_center_curve,
     fit_joint_image_curve,
     fit_measured_joint_curve,
@@ -30,6 +39,375 @@ from g20_thumb_apriltag_calibration.full_hand import (
 )
 
 
+def test_runtime_fit_saturates_at_source_urdf_limit() -> None:
+    values = tuple(np.linspace(0.0, 2.0, 256))
+    fit = JointCurveFit(
+        angle_rad=values,
+        decreasing_rad=values,
+        increasing_rad=values,
+        circle={},
+        maximum_monotonic_correction_rad=0.0,
+        maximum_hysteresis_rad=0.0,
+        quality={},
+    )
+
+    bounded = clamp_runtime_fits_to_urdf_limits(
+        Path(__file__).resolve().parents[3]
+        / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/"
+        "hands/linker_hand/g20_right/linkerhand_g20_right.urdf",
+        {"thumb_cmc_yaw": fit},
+    )["thumb_cmc_yaw"]
+
+    assert max(bounded.angle_rad) == pytest.approx(1.57)
+    assert max(bounded.decreasing_rad) == pytest.approx(1.57)
+    assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0
+
+
+def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None:
+    profile = get_hand_calibration_profile("right", "g20_right_15")
+    assert profile is RIGHT_19_HAND_PROFILE
+    assert len(profile.sweep_specs) == 16
+    assert len(profile.measured_joints) == 17
+    assert len(profile.record_joints) == 21
+    assert list(profile.view_tags["front"].values()) == [
+        0, 1, 2, 3, 10, 11, 12, 13
+    ]
+    assert list(profile.view_tags["side"].values()) == [
+        4, 5, 6, 15, 17
+    ]
+    assert list(profile.view_tags["top"].values()) == [8, 9]
+    assert profile.preflight_view_roles == {
+        "front": ("front_base",),
+        "side": ("side_base",),
+        "top": ("top_base",),
+    }
+    assert [spec.task_name for spec in profile.sweep_specs[:4]] == [
+        "thumb_cmc_pitch_front",
+        "thumb_cmc_roll_front",
+        "thumb_mcp_ip_front",
+        "thumb_cmc_yaw_top",
+    ]
+    assert [
+        spec.task_name for spec in profile.sweep_specs[4:7]
+    ] == [
+        "pinky_roll_multiview",
+        "pinky_pitch_side",
+        "pinky_pip_side",
+    ]
+    assert profile.sweep_specs[4].joints == (
+        "pinky_mcp_roll",
+        "pinky_mcp_roll_side",
+    )
+    assert {
+        name: spec.source_joint
+        for name, spec in profile.joint_specs.items()
+        if spec.source_joint is not None
+    } == {
+        "index_dip": "index_pip",
+        "middle_dip": "middle_pip",
+        "ring_dip": "ring_pip",
+        "pinky_dip": "pinky_pip",
+    }
+
+
+def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    values = tuple(0.002 * (255 - command) for command in range(256))
+    fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {})
+    measured = {name: fit for name in profile.measured_joints}
+    source_urdf = (
+        Path(__file__).resolve().parents[3]
+        / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+        "linker_hand/g20_right/linkerhand_g20_right.urdf"
+    )
+
+    runtime = derive_mimic_passive_fits(
+        source_urdf, measured, profile=profile
+    )
+
+    assert set(runtime) == set(profile.joint_specs)
+    for finger in ("index", "middle", "ring", "pinky"):
+        pip = runtime[f"{finger}_pip"]
+        dip = runtime[f"{finger}_dip"]
+        assert dip.angle_rad == pytest.approx(
+            tuple(0.89 * value for value in pip.angle_rad)
+        )
+        assert dip.maximum_hysteresis_rad == pytest.approx(0.0089)
+        assert dip.circle["curve_source"] == f"{finger}_pip"
+        assert dip.circle["visual_measurement"] is False
+
+
+def _changed_motors(
+    previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...]
+) -> list[int]:
+    return [
+        index
+        for index, (left, right) in enumerate(zip(previous, waypoint))
+        if left != right
+    ]
+
+
+def _phase_changes(
+    previous: tuple[int, ...] | list[int],
+    waypoints: tuple[tuple[int, ...], ...],
+) -> list[list[int]]:
+    phases: list[list[int]] = []
+    for waypoint in waypoints:
+        phases.append(_changed_motors(previous, waypoint))
+        previous = waypoint
+    return phases
+
+
+def test_right_19_side_clearance_waypoints_group_joint_classes() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs
+        if item.task_name == "index_roll_multiview"
+    )
+    baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+                255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=baseline,
+        profile=profile,
+    )
+    # Side clearance keeps all neighbouring roll axes neutral, flexes the
+    # neighbour PIPs together, then their pitches, and moves the index target
+    # last through the cleared swept volume.
+    assert _phase_changes(baseline, waypoints) == [
+        [17, 18, 19], [2, 3, 4], [6],
+    ]
+    assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255
+
+    returns = build_calibration_return_waypoints(
+        baseline,
+        current_command=waypoints[-1],
+        profile=profile,
+    )
+    assert _phase_changes(waypoints[-1], returns) == [
+        [6], [2, 3, 4], [17, 18, 19],
+    ]
+    assert list(returns[-1]) == baseline
+
+
+def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs
+        if item.task_name == "index_roll_multiview"
+    )
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+                         255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        profile=profile,
+        parallel=False,
+    )
+    previous = tuple([255] * 6 + [127] * 4 + [255] * 10)
+    changed_motors = []
+    for waypoint in waypoints:
+        changed = _changed_motors(previous, waypoint)
+        assert len(changed) == 1
+        changed_motors.append(changed[0])
+        previous = waypoint
+    # Side clearance keeps all neighbouring roll axes neutral, flexes them out
+    # of the swept volume, and moves the index target last.
+    assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6]
+    assert previous[1] == 255 and previous[16] == 255
+
+    returns = build_calibration_return_waypoints(
+        [255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+         255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        current_command=previous,
+        profile=profile,
+        parallel=False,
+    )
+    for waypoint in returns:
+        changed = sum(left != right for left, right in zip(previous, waypoint))
+        assert changed == 1
+        previous = waypoint
+
+
+def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs
+        if item.task_name == "ring_roll_multiview"
+    )
+    current = tuple(THREE_CAMERA_BASELINE_COMMAND)
+
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=current,
+        profile=profile,
+    )
+
+    assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]]
+    assert all(waypoint[9] == 127 for waypoint in waypoints)
+    assert waypoints[-1][8] == 255
+    assert waypoints[-1][19] == 0
+    assert waypoints[-1][4] == 0
+
+
+def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs
+        if item.task_name == "middle_roll_multiview"
+    )
+    # Session 20260819_160238 stopped here: the legacy front-only avoidance
+    # asked neighbouring ring roll (motor 8) for 0 and it saturated at 58.
+    current = [254] * 20
+    current[6:10] = [254, 127, 58, 127]
+    current[10] = 252
+    current[11:15] = [0, 0, 0, 0]
+
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=current,
+        profile=profile,
+    )
+
+    # Both differing roll axes (parked motor 6 and the stuck motor 8) move in
+    # the single roll phase, still never commanded toward 0.
+    phases = _phase_changes(current, waypoints)
+    assert phases[0] == [6, 8]
+    assert waypoints[0][8] == 127
+    assert all(
+        waypoint[motor] != 0
+        for waypoint in waypoints
+        for motor in range(6, 10)
+    )
+    assert waypoints[-1][6:10] == (255, 255, 127, 127)
+
+
+def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs
+        if item.task_name == "ring_pitch_side"
+    )
+    current = tuple(THREE_CAMERA_BASELINE_COMMAND)
+
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=current,
+        profile=profile,
+    )
+
+    assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]]
+    assert all(waypoint[8] == 127 for waypoint in waypoints)
+    assert all(waypoint[9] == 127 for waypoint in waypoints)
+    assert waypoints[-1][9] == 127
+    assert waypoints[-1][19] == 0
+    assert waypoints[-1][4] == 0
+
+
+def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None:
+    profile = RIGHT_19_HAND_PROFILE
+    spec = profile.sweep_specs[0]
+    # This is the real G20 feedback shape: reserved channels 11..14 report zero
+    # while active endpoint channels commonly settle one or two counts low.
+    current = [
+        255, 254, 254, 254, 254, 254, 127, 127, 127, 127,
+        253, 0, 0, 0, 0, 255, 253, 253, 253, 253,
+    ]
+    waypoints = build_calibration_preparation_waypoints(
+        spec,
+        255,
+        current_command=current,
+        profile=profile,
+    )
+    assert waypoints
+    assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints)
+    controlled = {joint.motor_index for joint in profile.joint_specs.values()}
+    for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints):
+        assert set(_changed_motors(previous, waypoint)) <= controlled
+
+    returns = build_calibration_return_waypoints(
+        list(THREE_CAMERA_BASELINE_COMMAND),
+        current_command=waypoints[-1],
+        profile=profile,
+    )
+    assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns)
+
+
+def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None:
+    # Full four-finger avoidance pose: rolls parked at 255 and every clearance
+    # finger flexed to 0.
+    current = list(THREE_CAMERA_BASELINE_COMMAND)
+    current[6:10] = [255, 255, 255, 255]
+    for motor in (1, 2, 3, 4, 16, 17, 18, 19):
+        current[motor] = 0
+    returns = build_calibration_return_waypoints(
+        list(THREE_CAMERA_BASELINE_COMMAND),
+        current_command=current,
+        profile=RIGHT_19_HAND_PROFILE,
+        anchor_roll_motor=9,
+    )
+    assert _phase_changes(current, returns) == [
+        [6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19],
+    ]
+    assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND)
+
+
+def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None:
+    current = list(THREE_CAMERA_BASELINE_COMMAND)
+    current[6:10] = [255, 255, 255, 255]
+    returns = build_calibration_return_waypoints(
+        list(THREE_CAMERA_BASELINE_COMMAND),
+        current_command=current,
+        profile=RIGHT_19_HAND_PROFILE,
+        anchor_roll_motor=9,
+        parallel=False,
+    )
+    phases = _phase_changes(current, returns)
+    assert all(len(phase) == 1 for phase in phases)
+    assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6]
+
+
+def test_right_19_ring_exit_returns_ring_before_neighbours() -> None:
+    current = list(THREE_CAMERA_BASELINE_COMMAND)
+    current[6:10] = [255, 255, 255, 0]
+    returns = build_calibration_return_waypoints(
+        list(THREE_CAMERA_BASELINE_COMMAND),
+        current_command=current,
+        profile=RIGHT_19_HAND_PROFILE,
+        anchor_roll_motor=8,
+        parallel=False,
+    )
+
+    first = returns[0]
+    assert first[8] == 127
+    assert first[6:8] == (255, 255)
+    assert first[9] == 0
+
+
+def test_cross_view_roll_curve_must_agree_before_fusion() -> None:
+    values = tuple(0.5 * (127 - command) / 127 for command in range(256))
+    fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
+    assert compare_cross_view_roll_curves(fit, fit)[
+        "angle_rad_rms_difference_rad"
+    ] == 0.0
+    bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values))
+    bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values,
+                  increasing_rad=bad_values)
+    with pytest.raises(ValueError, match="cross_view_roll_curve"):
+        compare_cross_view_roll_curves(fit, bad)
+
+    reverse = replace(
+        fit,
+        increasing_rad=tuple(value + math.radians(0.5) for value in values),
+    )
+    with pytest.raises(ValueError, match="branch_gap_difference"):
+        compare_cross_view_roll_curves(fit, reverse)
+
+
 def _records() -> list[dict[str, object]]:
     commands = list(range(0, 256, 16))
     if commands[-1] != 255:
@@ -498,3 +876,88 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
             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}
+
+
+def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> None:
+    from g20_thumb_apriltag_calibration.urdf_zero import (
+        get_zero_calibration_profile,
+    )
+
+    profile = RIGHT_19_HAND_PROFILE
+    zero = get_zero_calibration_profile("right", "g20_right_15")
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    fits = {}
+    diagnostics = {}
+    for name, spec in profile.joint_specs.items():
+        zero_command = baseline[spec.motor_index]
+        values = tuple(
+            0.002 * (zero_command - command) for command in range(256)
+        )
+        fits[name] = JointCurveFit(
+            values,
+            values,
+            values,
+            {},
+            0.0,
+            0.0,
+            {"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
+        )
+        diagnostics[name] = {
+            "cycle_travel_rad": [0.5, 0.5, 0.5, 0.5],
+            "cycle_travel_range_rad": 0.0,
+            "baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0],
+            "holdout_cycle": 3,
+            "holdout_cycle_mae_rad": 0.0,
+            "holdout_cycle_max_rad": 0.0,
+        }
+    offsets = {name: 0.0 for name in profile.active_joints}
+    payload = build_compact_payload(
+        serial_number="G20_RIGHT_019",
+        measured_fits=fits,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.0],
+        passed=True,
+        baseline=baseline,
+        side="right",
+        layout_id="g20_right_15",
+        zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
+        zero_cycle_offsets_rad={
+            name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
+        },
+        zero_observers=zero.offset_observer_joint,
+        artifact_hashes={
+            "source_urdf_sha256": "0" * 64,
+            "camera_extrinsics_sha256": "1" * 64,
+            "corrected_urdf_sha256": "2" * 64,
+        },
+        cross_view_roll_metrics={
+            f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
+            for finger in ("index", "middle", "ring", "pinky")
+        },
+        joint_dynamic_diagnostics=diagnostics,
+        zero_geometry_diagnostics={
+            "training_cycles": (0, 1, 2),
+            "validation_cycle": 3,
+            "axis_line_rms_m": 0.0,
+            "validation_line_error_by_joint_m": {},
+        },
+    )
+
+    validate_compact_payload(payload)
+    assert payload["schema_version"] == 4
+    assert set(payload) == {
+        "schema_version", "model", "side", "serial_number", "angle_unit",
+        "command_range", "baseline_command_u8", "joints", "quality",
+    }
+    assert len(zero.direct_zero_joints) == 12
+    assert payload["joints"]["thumb_mcp"]["zero_angles"] == {
+        "urdf_zero_offset_rad": 0.0
+    }
+    assert payload["joints"]["thumb_ip"]["passive"] is True
+    assert set(payload["joints"]["thumb_ip"]) == {
+        "motor_index", "angle_rad", "passive"
+    }
+    assert all(
+        "source_joint" not in joint for joint in payload["joints"].values()
+    )

src/g20_thumb_apriltag_calibration/test/test_pnp.py

@@ -234,6 +234,70 @@ def _pose(
     )
 
 
+def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("base", "mcp", "ip"),
+        adjacent_pairs=(("base", "mcp"), ("mcp", "ip")),
+        maximum_pose_jump_rad=np.deg2rad(360.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(1000.0),
+        relative_translation_scale_m=1.0,
+        reprojection_scale_px=0.1,
+        reprojection_weight=1.0,
+        reset_after_seconds=5.0,
+        coupled_rotation_pairs=(
+            ("base", "mcp", "mcp", "ip", 1.03),
+        ),
+        coupled_rotation_scale_rad=np.deg2rad(3.0),
+        maximum_coupled_rotation_residual_rad=np.deg2rad(7.5),
+    )
+    baseline = {
+        "base": (_pose(0.0, 0.00, 0.05),),
+        "mcp": (_pose(0.0, 0.03, 0.05),),
+        "ip": (_pose(0.0, 0.06, 0.05),),
+    }
+    selected, reason = tracker.select(
+        baseline,
+        stamp_ns=1_000_000_000,
+        trajectory_command_u8=255,
+        trajectory_direction="decreasing",
+    )
+    assert selected is not None
+    assert reason == ""
+
+    driver = _pose(10.0, 0.03, 0.05)
+    measured_ip = _pose(20.3, 0.06, 0.20)
+    lower_reprojection_mirror = _pose(30.0, 0.06, 0.05)
+    selected, reason = tracker.select(
+        {
+            "base": (_pose(0.0, 0.00, 0.05),),
+            "mcp": (driver,),
+            "ip": (lower_reprojection_mirror, measured_ip),
+        },
+        stamp_ns=1_033_000_000,
+        trajectory_command_u8=128,
+        trajectory_direction="decreasing",
+    )
+
+    assert reason == ""
+    assert selected is not None
+    assert selected["ip"] == measured_ip
+
+    fallback, fallback_reason = tracker.select(
+        {
+            "base": (_pose(0.0, 0.00, 0.05),),
+            "mcp": (driver,),
+            "ip": (lower_reprojection_mirror,),
+        },
+        stamp_ns=1_066_000_000,
+        trajectory_command_u8=128,
+        trajectory_direction="decreasing",
+    )
+    assert fallback is not None
+    assert fallback_reason == ""
+    assert fallback["ip"] == lower_reprojection_mirror
+
+
 def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
     tracker = SquareTagGroupPoseTracker(
         roles=("t0", "t3", "t4", "t5"),
@@ -324,6 +388,58 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
     assert selected == {"t4": return_t4, "t5": return_t5}
 
 
+def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> 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,
+    )
+    parent = _pose(0.0, 0.00, 0.05)
+    for stamp, command, angle in (
+        (1_000_000_000, 255, 20.0),
+        (1_033_000_000, 64, 45.0),
+        (1_066_000_000, 0, 50.0),
+    ):
+        selected, reason = tracker.select(
+            {"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)},
+            stamp_ns=stamp,
+            trajectory_command_u8=command,
+            trajectory_direction="decreasing",
+        )
+        assert reason == ""
+        assert selected is not None
+
+    # The true return contains 3 deg of real hysteresis along the learned
+    # y-axis.  The lower-error mirror candidate is temporally smoother but
+    # adds a 2 deg tilt outside that physical motion axis.
+    true_return = _pose(42.0, 0.03, 0.20)
+    smoother_mirror = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.03, 0.0, 0.25),
+        reprojection_error_px=0.01,
+    )
+    selected, reason = tracker.select(
+        {
+            "parent": (parent,),
+            "child": (smoother_mirror, true_return),
+        },
+        stamp_ns=1_099_000_000,
+        trajectory_command_u8=64,
+        trajectory_direction="increasing",
+    )
+
+    assert reason == ""
+    assert selected == {"parent": parent, "child": true_return}
+
+
 def test_group_tracker_initializes_from_multiple_static_frames() -> None:
     tracker = SquareTagGroupPoseTracker(
         roles=("parent", "child"),
@@ -372,6 +488,63 @@ def test_group_tracker_initializes_from_multiple_static_frames() -> None:
     ] < np.deg2rad(1.0)
 
 
+def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> 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)
+    anchored_child = _pose(20.0, 0.03, 0.20)
+    for index in range(8):
+        selected, reason = tracker.select(
+            {"parent": (parent,), "child": (anchored_child,)},
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+            trajectory_command_u8=255,
+            trajectory_direction="decreasing",
+        )
+    assert reason == ""
+    assert selected == {"parent": parent, "child": anchored_child}
+
+    tracker.reset(preserve_task_reference=True)
+    lower_error_mirror = _pose(5.0, 0.03, 0.01)
+    for index in range(8):
+        selected, reason = tracker.select(
+            {
+                "parent": (parent,),
+                "child": (lower_error_mirror, anchored_child),
+            },
+            stamp_ns=2_000_000_000 + index * 33_000_000,
+            trajectory_command_u8=255,
+            trajectory_direction="decreasing",
+        )
+
+    assert reason == ""
+    assert selected == {"parent": parent, "child": anchored_child}
+    assert tracker.last_initialization_quality["task_reference_used"] == "true"
+
+    tracker.reset()
+    for index in range(8):
+        selected, reason = tracker.select(
+            {
+                "parent": (parent,),
+                "child": (lower_error_mirror, anchored_child),
+            },
+            stamp_ns=3_000_000_000 + index * 33_000_000,
+            trajectory_command_u8=255,
+            trajectory_direction="decreasing",
+        )
+    assert reason == ""
+    assert selected == {"parent": parent, "child": lower_error_mirror}
+
+
 def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
     tracker = SquareTagGroupPoseTracker(
         roles=("mcp", "pip", "dip"),

src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py

@@ -74,6 +74,36 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
     assert "/g20_calibration/resume" in text
 
 
+def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
+    explanation, action = three_camera_reason_zh(
+        "PAUSED", "combination_pose_prediction_failed", {}
+    )
+
+    assert "多关节组合姿态" in explanation
+    assert "未分类原因码" not in explanation
+    assert "combination_validation_failure" in action
+    assert "不要重新采集16个单关节任务" in action
+
+
+def test_multiview_failure_names_the_camera_specific_joint() -> None:
+    explanation, action = three_camera_reason_zh(
+        "PAUSED",
+        "sweep_bin_gap_too_large:pinky_mcp_roll_side",
+        {
+            "sample": {
+                "maximum_bin_gap": 20,
+                "maximum_bin_gap_start_u8": 100,
+                "maximum_bin_gap_end_u8": 120,
+                "allowed_maximum_bin_gap": 16,
+            }
+        },
+    )
+
+    assert "小指MCP侧摆（侧面校验）" in explanation
+    assert "100→120" in explanation
+    assert "resume" in action
+
+
 def test_preflight_lists_missing_tags_in_chinese() -> None:
     payload = {
         "state": "PREFLIGHT",
@@ -137,6 +167,82 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
     assert "运动采样：" not in text
 
 
+def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "joint_fit_check_failed",
+        "progress": 0.15,
+        "completed_sweeps": 24,
+        "total_sweeps": 160,
+        "active": {
+            "kind": "fit_failure",
+            "view": "front",
+            "motor_index": 15,
+            "joints": ["thumb_ip"],
+            "attempt": 3,
+            "directions_to_rescan": 6,
+            "failures": [
+                {
+                    "joint": "thumb_ip",
+                    "metric": "baseline_hysteresis_deg",
+                    "actual": 1.34,
+                    "limit": 0.5,
+                    "comparison": "maximum",
+                    "cycle_values_deg": [1.34, 0.04, 0.14],
+                }
+            ],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "baseline正反程关节角差为1.34°" in text
+    assert "要求不超过0.50°" in text
+    assert "各轮=1.34°/0.04°/0.14°" in text
+    assert "未知原因" not in text
+
+
+def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "sweep_bin_gap_too_large",
+        "progress": 0.1,
+        "completed_sweeps": 1,
+        "total_sweeps": 10,
+        "active": {
+            "kind": "sweep",
+            "view": "top",
+            "motor_index": 10,
+            "joints": ["thumb_cmc_yaw"],
+            "cycle": 1,
+            "repetitions": 3,
+            "start_u8": 255,
+            "target_u8": 0,
+            "actual_u8": 3.0,
+            "valid_frames": 610,
+            "detection_frames": 644,
+            "detection_valid_frames": 610,
+            "detection_rate": 610 / 644,
+            "sample": {
+                "minimum_u8": 3.0,
+                "maximum_u8": 252.0,
+                "maximum_bin_gap": 17,
+                "maximum_bin_gap_start_u8": 99,
+                "maximum_bin_gap_end_u8": 116,
+                "allowed_maximum_bin_gap": 16,
+            },
+        },
+        "views": {},
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "最大空缺为17，（99→116）" in text
+    assert "本方向Tag检出：94.7%（610/644帧）" in text
+
+
 def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
     payload = {
         "state": "PAUSED",
@@ -210,6 +316,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
         "reason": (
             "motor_state_stalled:return_baseline:motor_index=10:"
             "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
+            "timeout_seconds=2.000:"
             "error_u8=5.000"
         ),
         "progress": 0.0,
@@ -224,6 +331,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
             "actual_u8": 250.0,
             "error_u8": 5.0,
             "tolerance_u8": 4.0,
+            "timeout_seconds": 2.0,
         },
         "views": {},
         "result_path": "",
@@ -231,7 +339,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
 
     text = render_three_camera_status_text_zh(payload)
 
-    assert "电机10反馈连续8秒" in text
+    assert "电机10反馈连续2秒" 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
@@ -371,3 +479,15 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
     assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
     assert "SDK报告[15, 5, 15, 15, 15]" in text
     assert "自动重试：当前方向已自动重扫1/2次" in text
+
+
+def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
+    explanation, action = three_camera_reason_zh(
+        "RETURN_BASELINE",
+        "holding_same_finger_clearance_before_next_task",
+        {},
+    )
+
+    assert "继续保持当前避让姿态" in explanation
+    assert "只调整被测关节" in explanation
+    assert "不要手动展开" in action

src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py

@@ -19,6 +19,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
     UrdfKinematicModel,
     _angles_from_state,
     _zero_sensitive_axis_error_rad,
+    baseline_hysteresis_by_cycle_rad,
     fit_joint_axis_measurement,
     fit_rotation_joint_curve,
     solve_urdf_zero_offsets,
@@ -157,6 +158,78 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
     assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
 
 
+def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None:
+    off_axis_noise = Rotation.from_rotvec(
+        np.radians([1.2, 0.0, 0.2])
+    ).as_quat().tolist()
+    records = []
+    for cycle in range(3):
+        records.extend(
+            (
+                {
+                    "cycle": cycle,
+                    "direction": "decreasing",
+                    "command_u8": 255,
+                    "relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
+                },
+                {
+                    "cycle": cycle,
+                    "direction": "increasing",
+                    "command_u8": 255,
+                    "relative_quaternion_xyzw": off_axis_noise,
+                },
+            )
+        )
+
+    full_pose = baseline_hysteresis_by_cycle_rad(
+        records, zero_command_u8=255
+    )
+    joint_angle = baseline_hysteresis_by_cycle_rad(
+        records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0]
+    )
+
+    assert math.degrees(full_pose[0]) == pytest.approx(
+        math.hypot(1.2, 0.2), abs=1.0e-9
+    )
+    assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9)
+
+
+def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None:
+    records = []
+    commands = sorted({0, 127, 255, *range(0, 256, 16)})
+    branch_offset = math.radians(1.0)
+    for cycle in range(3):
+        for direction in ("decreasing", "increasing"):
+            offset = branch_offset if direction == "increasing" else 0.0
+            for command in commands:
+                angle = math.radians(50.0) * (127.0 - command) / 255.0
+                angle += offset
+                rotation = Rotation.from_rotvec([0.0, 0.0, angle])
+                records.append(
+                    {
+                        "cycle": cycle,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_quaternion_xyzw": rotation.as_quat().tolist(),
+                    }
+                )
+
+    curve = fit_rotation_joint_curve(
+        records,
+        zero_command_u8=127,
+        canonical_zero_direction="decreasing",
+    )
+
+    assert curve.angle_rad == curve.decreasing_rad
+    assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9)
+    assert curve.increasing_rad[127] == pytest.approx(
+        branch_offset, abs=1.0e-6
+    )
+    assert curve.maximum_hysteresis_rad == pytest.approx(
+        branch_offset, 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,
@@ -234,6 +307,35 @@ def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
     ) < 1.0e-6
 
 
+def test_side_roll_axis_point_ignores_real_screw_translation() -> 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)
+    screw_records = []
+    for record in records:
+        changed = dict(record)
+        fraction = (255.0 - float(record["command_u8"])) / 255.0
+        changed["relative_translation_xyz_m"] = (
+            np.asarray(record["relative_translation_xyz_m"], dtype=float)
+            + 0.008 * fraction * axis_parent
+        ).tolist()
+        screw_records.append(changed)
+
+    measurement = fit_joint_axis_measurement(
+        "middle_mcp_roll_side",
+        screw_records,
+        cycle=0,
+        zero_command_u8=255,
+        constrained_circle_joints=frozenset({"middle_mcp_roll_side"}),
+    )
+
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    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)
@@ -479,15 +581,17 @@ def _solve_synthetic_offsets(
     side: str,
     offset_degrees: list[float],
     *,
+    layout_id: str = "legacy_11",
     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,
+    validation_offset_bias_degrees: dict[str, float] | None = None,
 ):
-    hand = get_hand_calibration_profile(side)
+    hand = get_hand_calibration_profile(side, layout_id)
-    zero = get_zero_calibration_profile(side)
+    zero = get_zero_calibration_profile(side, layout_id)
     source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
     baseline = [255.0] * 20
     baseline[6:10] = [127.0] * 4
@@ -517,7 +621,9 @@ def _solve_synthetic_offsets(
     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):
+    validation_cycle = 3 if layout_id == "g20_right_15" else 2
+    training_cycles = tuple(range(validation_cycle))
+    for cycle in range(validation_cycle + 1):
         for joint in zero.axis_joints:
             state = list(baseline)
             if joint == "thumb_cmc_yaw":
@@ -528,8 +634,18 @@ def _solve_synthetic_offsets(
                 motor_by_joint=motor_by_joint,
                 inherited_zero_joints=zero.inherited_zero_joints,
             )
+            cycle_offsets = dict(offsets)
+            if cycle == validation_cycle:
+                cycle_offsets.update(
+                    {
+                        name: cycle_offsets[name] + math.radians(value)
+                        for name, value in (
+                            validation_offset_bias_degrees or {}
+                        ).items()
+                    }
+                )
             axis, point = model.axis_line(
-                joint, zero_offsets=offsets, joint_angles=angles
+                joint, zero_offsets=cycle_offsets, joint_angles=angles
             )
             point_common = base_rotation.apply(point) + base_translation
             if (
@@ -622,14 +738,229 @@ def _solve_synthetic_offsets(
         curves=curves,
         motor_by_joint=motor_by_joint,
         hand_type=side,
+        tag_layout=layout_id,
         joint_maximum_offset_rad={
             name: math.radians(value)
             for name, value in (joint_maximum_offset_degrees or {}).items()
         },
+        training_cycles=training_cycles,
+        validation_cycle=validation_cycle,
     )
     return zero, result
 
 
+def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None:
+    injected = [
+        2.0, -3.0, 4.0, -1.5,
+        1.0, -1.0,
+        0.8, -0.7,
+        -0.5, 0.6,
+        1.1, -1.0,
+    ]
+    zero, result = _solve_synthetic_offsets(
+        "right", injected, layout_id="g20_right_15"
+    )
+
+    assert len(zero.direct_zero_joints) == 12
+    assert len(zero.axis_joints) == 17
+    assert result.passed is True
+    finger_rolls = tuple(
+        name
+        for name in zero.direct_zero_joints
+        if name.endswith("_mcp_roll") and not name.startswith("thumb_")
+    )
+    roll_common = float(
+        np.median(
+            [
+                injected[zero.direct_zero_joints.index(name)]
+                for name in finger_rolls
+            ]
+        )
+    )
+    for name, expected in zip(zero.direct_zero_joints, injected):
+        if name == "thumb_mcp":
+            expected = 0.0
+        elif name in finger_rolls:
+            expected -= roll_common
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            expected, abs=0.05
+        )
+    assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
+        0.0, abs=0.05
+    )
+    assert result.training_cycles == (0, 1, 2)
+    assert result.validation_cycle == 3
+    assert all(
+        value == pytest.approx(0.0, abs=1.0e-12)
+        for value in result.offset_confidence_half_width_rad.values()
+    )
+    assert all(
+        result.all_active_offsets_rad[f"{finger}_pip"] == 0.0
+        for finger in ("index", "middle", "ring", "pinky")
+    )
+    assert result.observability_parameter_count == 18
+    assert result.observability_rank == 18
+    assert math.isfinite(result.observability_condition_number)
+    assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
+
+
+def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
+    # A shared electrical centre belongs to the four-motor common datum; the
+    # strict 3 deg assembly guard applies to finger-to-finger deviations.
+    injected = [
+        2.0, -3.0, 4.0, -1.5,
+        6.0, -1.0,
+        6.4, -0.7,
+        5.7, 0.6,
+        6.2, -1.0,
+    ]
+
+    zero, result = _solve_synthetic_offsets(
+        "right", injected, layout_id="g20_right_15"
+    )
+
+    assert result.passed is True
+    finger_rolls = tuple(
+        name
+        for name in zero.direct_zero_joints
+        if name.endswith("_mcp_roll") and not name.startswith("thumb_")
+    )
+    roll_common = float(
+        np.median(
+            [
+                injected[zero.direct_zero_joints.index(name)]
+                for name in finger_rolls
+            ]
+        )
+    )
+    for name, expected in zip(zero.direct_zero_joints, injected):
+        if name == "thumb_mcp":
+            expected = 0.0
+        elif name in finger_rolls:
+            expected -= roll_common
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            expected, abs=0.05
+        )
+    assert np.median(
+        [result.direct_offsets_rad[name] for name in finger_rolls]
+    ) == pytest.approx(0.0, abs=1.0e-10)
+
+
+def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
+    injected = [
+        2.0, -3.0, 4.0, -1.5,
+        1.0, -1.0,
+        0.8, -0.7,
+        -0.5, 0.6,
+        1.1, -1.0,
+    ]
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        injected,
+        layout_id="g20_right_15",
+        validation_offset_bias_degrees={"thumb_cmc_roll": 2.0},
+    )
+
+    assert math.degrees(
+        result.direct_offsets_rad["thumb_cmc_roll"]
+    ) == pytest.approx(injected[0], abs=0.05)
+    assert result.validation_cycle not in result.training_cycles
+
+
+def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> None:
+    """Guard the reviewed 6-palm-DOF plus 12-static-zero observability."""
+    zero = get_zero_calibration_profile("right", "g20_right_15")
+    model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
+    parameter_count = 6 + len(zero.direct_zero_joints)
+
+    def line_observations(parameters: np.ndarray) -> np.ndarray:
+        palm_rotation = Rotation.from_rotvec(parameters[:3])
+        palm_translation = parameters[3:6]
+        offsets = {
+            name: float(value)
+            for name, value in zip(
+                zero.direct_zero_joints, parameters[6:]
+            )
+        }
+        result: list[float] = []
+        for joint in zero.axis_joints:
+            axis, point = model.axis_line(
+                joint, zero_offsets=offsets, joint_angles={}
+            )
+            axis = palm_rotation.apply(axis)
+            point = palm_rotation.apply(point) + palm_translation
+            # An oriented 3-D line is represented by its direction and
+            # Pluecker moment.  The moment is invariant to choosing a different
+            # point along the same axis, so no unobservable along-axis Tag
+            # placement is accidentally counted as information.
+            result.extend(float(value) for value in axis)
+            result.extend(float(value) for value in np.cross(point, axis))
+        return np.asarray(result, dtype=float)
+
+    origin = np.zeros(parameter_count, dtype=float)
+    step = 1.0e-6
+    jacobian = np.column_stack(
+        [
+            (
+                line_observations(
+                    origin + np.eye(parameter_count, dtype=float)[index] * step
+                )
+                - line_observations(
+                    origin - np.eye(parameter_count, dtype=float)[index] * step
+                )
+            )
+            / (2.0 * step)
+            for index in range(parameter_count)
+        ]
+    )
+
+    assert parameter_count == 18
+    assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count
+
+
+def test_right_15_urdf_writer_changes_only_the_12_static_targets(
+    tmp_path: Path,
+) -> None:
+    zero = get_zero_calibration_profile("right", "g20_right_15")
+    offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints}
+    destination = write_zero_corrected_urdf(
+        source_urdf=RIGHT_SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="G20_RIGHT_019",
+        offsets_rad=offsets,
+        timestamp="20260818_120000",
+    )
+
+    original = {
+        str(joint.get("name")): joint
+        for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint")
+    }
+    corrected = {
+        str(joint.get("name")): joint
+        for joint in ET.parse(destination).getroot().findall("joint")
+    }
+    for mesh in ET.parse(destination).getroot().findall(".//mesh"):
+        relative = Path(mesh.get("filename"))
+        copied = destination.parent / relative
+        source = RIGHT_SOURCE_URDF.parent / relative
+        assert copied.is_file()
+        assert copied.stat().st_size == source.stat().st_size
+    changed = set()
+    for name in original:
+        original_origin = original[name].find("origin")
+        corrected_origin = corrected[name].find("origin")
+        if original_origin is None or corrected_origin is None:
+            continue
+        if original_origin.get("rpy") != corrected_origin.get("rpy"):
+            changed.add(name)
+        assert original_origin.get("xyz") == corrected_origin.get("xyz")
+    assert changed == set(zero.direct_zero_joints)
+    assert "thumb_mcp" in changed
+    assert not set(get_hand_calibration_profile(
+        "right", "g20_right_15"
+    ).passive_joints) & changed
+
+
 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
@@ -712,7 +1043,7 @@ def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None
 
     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 result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((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