拇指侧摆零位标定

.gitignore

@@ -60,11 +60,15 @@ Thumbs.db
 # Reproducible seed profiles remain under
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
+/calibration_output/
 *.wear_check.json
 *.checkpoint.json
 *.verification.json
 *_mapping_quality.json
 
+# Device-specific robot descriptions derived from local CMC pitch calibration
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
+
 # ROS bag / MCAP recordings and CAN captures
 rosbag2_*/
 /bags/

src/g20_thumb_apriltag_calibration/README.md

@@ -0,0 +1,398 @@
+# G20 左手拇指正面 AprilTag 标定
+
+该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
+只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
+终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
+程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
+完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点
+平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。
+
+当前默认使用 `trajectory_center_3d`。节点由四个亚像素角点和 `CameraInfo.P`
+计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角：
+
+- 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴；
+  每帧三个角度取中位数。
+- 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入，
+  URDF 将被动 IP 定义为 `thumb_ip = 1.02 × thumb_mcp`，因此运行时 IP 曲线严格按
+  这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP
+  真实运动。
+- 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于
+  `trajectory_center_quality.tip` 中的观测一致性诊断，不参与最终 IP 数组。
+- 每条曲线都减去命令 255 的测量角，所以最终文件严格满足
+  `angle_rad[255] == 0.0`；`angle_rad[0]` 是该关节相对零位的最大角度。
+
+这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。
+但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会
+检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。
+
+PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再
+直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支；
+尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停，
+避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。
+
+## 1. 标记和安全检查
+
+- `T0` 必须保留并固定在掌壳，作为整体平移参考；`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆，
+  `T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
+- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
+  `config/front_tags.yaml` 里检测节点和标定节点的两组数组。
+- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前配置为 `0.010`。
+  测量检测角点所围成的正方形边长，不包含外围白色留边。
+- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
+  静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
+  启动参数增加 `publish_debug_image:=true`，再订阅
+  `/g20_thumb_calibration/debug_image`；正式长时间扫描建议保持默认关闭。
+- 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15
+  均可安全走完整 `255→0→255`。标定节点发现命令话题上另有发布者时不会解锁扫描。
+
+## 2. 安装与构建
+
+```bash
+sudo apt-get update
+sudo apt-get install -y \
+  ros-jazzy-realsense2-camera \
+  ros-jazzy-realsense2-description \
+  ros-jazzy-image-pipeline \
+  ros-jazzy-apriltag-ros \
+  ros-jazzy-apriltag-msgs
+
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+colcon build --symlink-install \
+  --packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
+source install/setup.bash
+```
+
+连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
+`/g20/cb_left_hand_control_cmd` 的程序。
+
+## 3. 启动和操作
+
+首次使用时可先用 `commands_enabled:=false` 做预检；SDK 仍会设置速度/扭矩并读取状态，
+但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+  serial_number:=G20_LEFT_001 \
+  commands_enabled:=false
+```
+
+确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后，
+停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 `decimate=1.5` 提升检测
+速度，并使用单终点连续运动：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+  serial_number:=G20_LEFT_001 \
+  can_interface:=can0 \
+  calibration_speed:=15 \
+  continuous_motion_mode:=endpoint \
+  angle_estimation_mode:=trajectory_center_3d \
+  apriltag_decimate:=1.5 \
+  use_roi:=false
+```
+
+默认关闭 ROI，AprilTag 使用完整的 1280×720 校正画面。查看实际送入 AprilTag
+的完整画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/camera/camera/color/image_rect
+```
+
+图像检测链路使用 `sensor_data`（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率
+下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。
+
+若以后需要以帧率优先，可传入 `use_roi:=true`；默认 ROI 是原图中的
+`x=128, y=192, width=1024, height=528`，也可用 `roi_x`、`roi_y`、
+`roi_width`、`roi_height` 覆盖。
+
+监控状态：
+
+```bash
+ros2 topic echo /g20_thumb_calibration/status
+```
+
+预检通过后状态为 `WAIT_ROOT_CONFIRM`，`reason` 为 `call_start`。只需调用一次：
+
+```bash
+ros2 service call /g20_thumb_calibration/start std_srvs/srv/Trigger {}
+```
+
+节点随后自动完成下标 0 的 `255→0→255`、下标 15 的 `255→0→255` 和 5 点随机复测，
+正常结束状态为 `COMPLETE`，无需在根部和尖部之间再次确认。为安全起见，调用 `start`
+前必须一次性确认两个关节的完整行程都已清空。原来的
+`confirm_root_full_range`、`confirm_tip_full_range` 服务仍保留用于兼容。
+
+暂停、恢复和终止：
+
+```bash
+ros2 service call /g20_thumb_calibration/pause std_srvs/srv/Trigger {}
+ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {}
+ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}
+```
+
+预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须
+至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心
+的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动
+只作为诊断，不会阻止静态捕获。
+状态中的
+`pnp_rejections` 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝，
+`pnp_reprojection_error_px` 显示四张 Tag 最近一次有效解的误差。连续扫描要求
+图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、
+至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、
+90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到
+该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列，
+不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。
+
+PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测
+间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定
+窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停，
+不会再生成明知不可靠但字段完整的结果。
+
+单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用
+实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待
+电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路，
+不要降低到固件低速区。必须临时回退时可启动
+`continuous_motion_mode:=paced`，该模式按步长 8 到位即发下一段。
+
+连续扫描中的主要状态字段：
+
+- `state_zh`/`reason_zh`/`action_zh`：当前阶段、失败原因和下一步操作的中文说明；
+  原有 `state`/`reason` 英文机器码继续保留。
+- `tag_quality`：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、
+  是否有效和具体中文问题，不再需要手工解析 `/apriltag/detections`。
+- `/g20_thumb_calibration/status_text`：适合终端直接查看的多行中文状态。使用
+  `ros2 topic echo --once /g20_thumb_calibration/status_text --field data`
+  即可看到原因、建议及四张标签的质量。
+- `scan_progress`：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。
+- `sweep_valid_frames_seen`：当前连续方向已收到的同步有效帧数。
+- `sweep_state_span_u8`：当前方向实际覆盖的电机范围，接近 255 才算完整。
+- `active_phase`/`active_direction`：当前是根部或尖部、下降或上升方向。
+- `pnp_branch_corrections`：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择
+  单张 Tag 最小重投影分支的累计次数。
+- `pnp_trajectory_quality`：最近一个完整方向的整段分支修正帧数，以及相对整段稳健
+  参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断：
+  P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移
+  只保留为诊断。
+- `trajectory_center_quality`：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、
+  实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中
+  `tip.ip_observed_vs_constrained_*` 显示T5残余小圆与URDF被动耦合之间的差异；
+  它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。
+
+根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解；
+尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。
+
+## 4. 中断恢复和输出
+
+默认会话目录是启动命令当前目录下：
+
+```text
+calibration_output/<序列号>/<时间戳>/
+```
+
+恢复时必须显式复用原目录，否则会创建新会话：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+  serial_number:=G20_LEFT_001 \
+  session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
+```
+
+恢复会校验序列号、Tag 配置、基准命令、扫描模式、采集参数和代码哈希；
+任一项变化都会拒绝混用旧样本，
+此时应新建会话。
+
+目录内文件：
+
+- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个
+  扫描方向后落盘。
+- `checkpoint.json`：当前状态和进度。
+- `session_manifest.json`：Tag、相机内参、SDK、代码哈希和会话信息。
+- `validation.json`：随机复测及全部质量判据。
+- `rosbag/`：仅在 `record_bag:=true` 时生成，用于保存相机、检测、命令和状态等诊断数据。
+- `g20_left_<序列号>_thumb_angle.json`：精简后的运行时标定文件。
+
+最终文件使用 `schema_version: 2`。每个关节只包含：
+
+```json
+{
+  "motor_index": 0,
+  "angle_rad": ["按命令0～255索引的256个弧度值"]
+}
+```
+
+`thumb_ip.angle_rad` 由 `thumb_mcp.angle_rad` 乘
+`ip_coupling.multiplier`（默认 `1.02`）得到，二者在命令255处都严格为零。
+
+`thumb_ip` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留
+在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。
+
+零位和最大角度可直接读取：
+
+```python
+import json
+from pathlib import Path
+
+data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text())
+for name, joint in data["joints"].items():
+    print(name, "zero(rad)=", joint["angle_rad"][255],
+          "max(rad)=", joint["angle_rad"][0])
+```
+
+如果相机或 SDK 已由外部进程启动，可传
+`start_camera:=false` 或 `start_sdk:=false`。用 `camera_serial_number:=<序列号>`
+可绑定指定 RealSense。
+
+D405 的彩色流来自 `depth_module`，启动文件会同时设置
+`depth_module.color_profile` 和 `rgb_camera.color_profile`，默认均为
+`1280x720x30`。连续模式默认关闭深度和 rosbag，以减少 USB、CPU 和磁盘负担；
+它们都不参与角度计算。需要完整诊断留档时可增加
+`enable_depth:=true record_bag:=true`。
+
+默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
+`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
+校正和 AprilTag 组件运行
+在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
+可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
+不订阅整幅图像，只订阅检测结果和 TF。
+若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
+不影响 AprilTag 的 ROI 输入。
+
+静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
+当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
+相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
+
+启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
+订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
+若用于正式可追溯验收，再启用并检查磁盘空间。
+
+## 5. CMC Pitch 零位角测量
+
+只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。
+它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改
+URDF：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  front_cmc_pitch_zero.launch.py \
+  serial_number:=G20_LEFT_001
+```
+
+该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。
+预检完成后查看中文状态：
+
+```bash
+ros2 topic echo --once --full-length \
+  /g20_thumb_cmc_pitch_zero/status_text \
+  --field data
+```
+
+状态显示“等待开始”后启动三轮测量：
+
+```bash
+ros2 service call \
+  /g20_thumb_cmc_pitch_zero/start \
+  std_srvs/srv/Trigger {}
+```
+
+查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线
+的调试画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_thumb_cmc_pitch_zero/debug_image
+```
+
+画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、
+接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线
+重合；画面和 `status_text` 会实时显示红蓝线夹角及垂直偏差。`±0.5°` 和
+`±12 px` 只用于显示 `ALIGNED/ADJUST`，完全不参与预检或 `start` 服务判断。
+由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面；
+该检测不使用 T0/T3 标签朝向。
+
+一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、
+偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。
+
+每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间
+连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用
+“T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位，
+三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布
+`/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。
+
+T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 `+x`
+都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、
+固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格，
+节点暂停或写出 `quality.passed=false`。
+
+完成后只生成：
+
+```text
+calibration_output/G20_LEFT_001/<时间戳>/
+  g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json
+```
+
+核心字段是：
+
+```text
+zero_angles.table_projected_zero_rad
+```
+
+该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向，
+但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手
+摆放改变。
+
+## 6. CMC Roll 零位与行程标定
+
+`thumb_cmc_roll` 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合，
+但控制的是电机 5。每轮执行 `255→0→255`：在 255 零位、0 行程端点和返回
+255 后各静态采集 30 帧，因此可以同时测量零位角和完整 `0～255` 实际角行程。
+命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。
+Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  front_cmc_roll_calibration.launch.py \
+  serial_number:=G20_LEFT_001
+```
+
+预检通过后启动三轮标定：
+
+```bash
+ros2 topic echo --once --full-length \
+  /g20_thumb_cmc_roll_calibration/status_text \
+  --field data
+
+ros2 service call \
+  /g20_thumb_cmc_roll_calibration/start \
+  std_srvs/srv/Trigger {}
+```
+
+调试画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_thumb_cmc_roll_calibration/debug_image
+```
+
+Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用
+蓝线状态阻止 `start` 进入电机运动。
+
+完成后生成：
+
+```text
+calibration_output/G20_LEFT_001/<时间戳>/
+  g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json
+```
+
+核心输出字段：
+
+```text
+zero_angles.table_projected_zero_rad
+travel.signed_rad
+travel.range_rad
+```
+
+`travel.signed_rad` 是从命令 255 到 0 的有符号转角，`travel.range_rad` 是三轮
+行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向
+误差和回零误差全部通过时，`quality.passed` 才为 `true`。

src/g20_thumb_apriltag_calibration/config/calibration.yaml

@@ -0,0 +1,136 @@
+g20_thumb_calibration:
+  ros__parameters:
+    command_topic: /g20/cb_left_hand_control_cmd
+    state_topic: /g20/cb_left_hand_state
+    info_topic: /g20/cb_left_hand_info
+    camera_info_topic: /camera/camera/color/camera_info
+    image_topic: /camera/camera/color/image_rect
+    detections_topic: /apriltag/detections
+    tf_topic: /tf
+    # Use PnP translations as 3-D Tag centres and fit the directly observable
+    # root/MCP circles. The passive IP output follows the G20 URDF mimic
+    # relation below; its small residual T5 circle is diagnostic only. PnP
+    # orientations remain auxiliary quality checks. Command 255 is zero.
+    angle_estimation_mode: trajectory_center_3d
+    passive_ip_multiplier: 1.02
+    publish_debug_image: false
+    debug_max_rate_hz: 10.0
+    debug_scale: 0.5
+
+    # Default: send one end-to-end command per direction and pair every valid
+    # AprilTag frame with the timestamp-interpolated actual G20 state.
+    scan_mode: continuous
+    continuous_motion_mode: endpoint
+    repetitions: 1
+    # Used only by point-mode fallback and validation approach offsets.
+    command_step: 8
+    auto_start_tip: true
+    maximum_state_image_skew_ms: 150.0
+    continuous_endpoint_tolerance_u8: 2.0
+    continuous_endpoint_hold_seconds: 1.0
+    continuous_timeout_seconds: 90.0
+    continuous_invalid_timeout_seconds: 3.0
+    continuous_minimum_valid_frames: 40
+    continuous_minimum_state_span_u8: 240.0
+    continuous_minimum_bins: 32
+    continuous_maximum_bin_gap: 16
+    # Keep the responsive firmware speed, but pace it through the same
+    # 8-unit grid without waiting for static image captures at each point.
+    continuous_segment_minimum_seconds: 0.1
+    continuous_segment_timeout_seconds: 5.0
+    continuous_prepare_timeout_seconds: 30.0
+    preflight_frames: 150
+    minimum_detection_rate: 0.95
+    minimum_detection_hz: 15.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    # Trial threshold for the current 10 mm tags (observed at 32-38 px).
+    # Final acceptance is still guarded by static RMS and random validation.
+    minimum_edge_pixels: 30.0
+    # Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
+    # Keep a small practical margin here; final random validation stays at
+    # MAE <= 2 deg and P95 <= 3 deg.
+    # Match the preflight noise gate to the 3 deg robust capture gate below.
+    # The final calibration is still accepted only by the independent
+    # validation MAE/P95 limits, not by this readiness check.
+    maximum_static_std_deg: 3.0
+    pose_outlier_threshold_deg: 5.0
+    minimum_pose_inlier_rate: 0.90
+    pnp_minimum_valid_rate: 0.95
+    # 30-38 px tags are usable, but only if IPPE gives a tight image fit and
+    # a pose continuous with the preceding frame.
+    pnp_maximum_reprojection_error_px: 1.5
+    # All four tags keep a temporally continuous IPPE solution throughout the
+    # complete session. With 30 px planar tags, tiny reprojection differences
+    # do not reliably identify the physical branch and previously caused
+    # stationary T0 to flip by about 25 deg between scan and validation.
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    # Preserve the branch through short detector gaps. A continuous sweep
+    # already pauses after 3 s without valid synchronised observations.
+    pnp_tracker_reset_seconds: 5.0
+    # Select all four IPPE branches as one kinematic chain. This prevents T4
+    # and T5 from independently changing mirror branches at the turnaround or
+    # during validation while still allowing real joint motion frame-to-frame.
+    pnp_group_relative_rotation_scale_deg: 5.0
+    pnp_group_relative_translation_scale_m: 0.01
+    # Reprojection remains a tie-breaker; temporal joint-chain continuity is
+    # deliberately dominant for the current 30-38 px planar tags.
+    pnp_group_reprojection_weight: 0.05
+    # Whole-sweep branch review. During a root sweep T3/T4/T5 should retain
+    # rigid relative poses; during a tip sweep T0/T3 should remain fixed.
+    pnp_trajectory_reprojection_scale_px: 0.1
+    pnp_rigid_rotation_scale_deg: 5.0
+    pnp_rigid_translation_scale_m: 0.01
+    # Judge the complete rigid trajectory against a robust sweep reference.
+    # Reject persistent drift at P95; keep a looser hard maximum so one noisy
+    # 30 px endpoint frame does not discard an otherwise sound sweep.
+    pnp_rigid_p95_accepted_drift_deg: 8.0
+    pnp_rigid_maximum_accepted_drift_deg: 15.0
+    # Centre-trajectory mode judges branch consistency by the Euclidean
+    # distance between rigid Tag centres.  This is deliberately independent
+    # of the noisy planar-Tag orientation returned by PnP.
+    pnp_rigid_p95_accepted_distance_drift_m: 0.003
+    pnp_rigid_maximum_accepted_distance_drift_m: 0.006
+
+    # Three-dimensional centre-trajectory geometry gates. T0 stays on the
+    # palm as the translation anchor; T3/T4/T5 are the moving thumb points.
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.005
+    trajectory_minimum_arc_deg: 15.0
+    trajectory_maximum_root_role_disagreement_deg: 5.0
+    trajectory_maximum_anchor_drift_m: 0.005
+    trajectory_static_translation_outlier_m: 0.005
+    trajectory_maximum_static_translation_rms_m: 0.002
+
+    # Static captures are now used only for sweep preparation and validation.
+    stable_frames: 5
+    capture_frames: 8
+    minimum_settle_seconds: 0.4
+    # This only confirms that the hand has stopped before an 8-frame robust
+    # median capture. The passive T4->T5 pair currently has about 2.3 deg
+    # peak spread over five 30 px PnP frames, while its two IPPE branches are
+    # separated by about 5.5 deg. A 3 deg gate accepts measurement jitter but
+    # still rejects a branch change. Final MAE/P95 limits remain unchanged.
+    maximum_stable_spread_deg: 3.0
+    # In centre mode the stationary capture gate is expressed in metres.
+    maximum_stable_translation_spread_m: 0.003
+    settle_timeout_seconds: 10.0
+    capture_timeout_seconds: 10.0
+
+    validation_command_count: 5
+    # The backlash approach point only waits for feedback to reach the target;
+    # it no longer performs an unnecessary image capture.
+    validation_approach_minimum_seconds: 0.2
+    validation_approach_timeout_seconds: 10.0
+    validation_position_tolerance_u8: 2.0
+    validation_seed: 20260727
+    maximum_validation_mae_deg: 2.0
+    maximum_validation_p95_deg: 3.0
+    maximum_coupling_drift_deg: 2.0
+    minimum_ip_coupling_r_squared: 0.98
+    maximum_monotonic_correction_deg: 2.0
+    maximum_hysteresis_deg: 5.0

src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml

@@ -0,0 +1,59 @@
+g20_thumb_cmc_pitch_zero:
+  ros__parameters:
+    t0_id: 0
+    t3_id: 1
+    joint_name: thumb_cmc_pitch
+    motor_index: 0
+    zero_command_u8: 255
+    measure_travel: false
+    baseline_command_u8:
+      [255, 255, 255, 255, 255, 255, 193, 148, 105, 42,
+       245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    repetitions: 3
+    zero_capture_frames: 30
+    # Each round sends one 255->64 endpoint command and one 64->255 return
+    # command.  All valid T3-minus-T0 centres observed during both motions are
+    # state-binned and fitted to one image-plane circle.
+    trajectory_command_u8: 64
+    trajectory_bin_size_u8: 8.0
+    trajectory_minimum_frames: 45
+    trajectory_minimum_bins: 18
+    trajectory_minimum_state_span_u8: 160.0
+    trajectory_minimum_radius_px: 20.0
+    trajectory_minimum_arc_deg: 20.0
+    trajectory_maximum_radial_rms_px: 2.0
+    trajectory_maximum_p95_radial_error_px: 3.5
+    trajectory_endpoint_settle_seconds: 0.3
+    trajectory_timeout_seconds: 30.0
+    settle_seconds: 0.5
+    move_timeout_seconds: 20.0
+    capture_timeout_seconds: 15.0
+    state_tolerance_u8: 2.0
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 40.0
+    maximum_static_position_rms_px: 1.5
+    maximum_round_difference_deg: 1.0
+    maximum_return_error_deg: 1.0
+    # Zero direction is always T3 centre -> fitted circle centre. T3's printed
+    # orientation and corner +x direction are deliberately not used.
+    maximum_zero_radial_error_px: 4.0
+    # Detect a long physical table/reference edge in the lower image. The red
+    # target and blue detected line are display-only aids for manual alignment;
+    # they never block preflight or the start service.
+    camera_alignment_enabled: true
+    camera_alignment_reference_y_ratio: 0.90
+    camera_alignment_roi_y_min_ratio: 0.55
+    camera_alignment_roi_y_max_ratio: 0.98
+    camera_alignment_minimum_line_length_ratio: 0.30
+    camera_alignment_max_candidate_angle_deg: 15.0
+    camera_alignment_max_angle_deg: 0.5
+    camera_alignment_max_vertical_offset_px: 12.0
+    camera_alignment_required_frames: 10
+    camera_alignment_minimum_detection_rate: 0.8
+    camera_alignment_max_age_seconds: 1.0
+    publish_debug_image: true
+    debug_max_rate_hz: 10.0
+    debug_scale: 0.75

src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml

@@ -0,0 +1,60 @@
+g20_thumb_cmc_roll_calibration:
+  ros__parameters:
+    t0_id: 0
+    t3_id: 1
+    joint_name: thumb_cmc_roll
+    motor_index: 5
+    zero_command_u8: 255
+    measure_travel: true
+    baseline_command_u8:
+      [255, 255, 255, 255, 255, 255, 193, 148, 105, 42,
+       245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    repetitions: 3
+    zero_capture_frames: 30
+    # Measure the complete motor-5 range. Each round captures both static
+    # endpoints around one 255->0->255 circle trajectory.
+    trajectory_command_u8: 0
+    trajectory_bin_size_u8: 8.0
+    trajectory_minimum_frames: 65
+    trajectory_minimum_bins: 30
+    trajectory_minimum_state_span_u8: 240.0
+    trajectory_minimum_radius_px: 20.0
+    trajectory_minimum_arc_deg: 20.0
+    trajectory_maximum_radial_rms_px: 2.0
+    trajectory_maximum_p95_radial_error_px: 3.5
+    trajectory_endpoint_settle_seconds: 0.3
+    trajectory_timeout_seconds: 35.0
+    settle_seconds: 0.5
+    move_timeout_seconds: 25.0
+    capture_timeout_seconds: 15.0
+    state_tolerance_u8: 2.0
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 40.0
+    maximum_static_position_rms_px: 1.5
+    maximum_round_difference_deg: 1.0
+    maximum_travel_difference_deg: 1.0
+    minimum_travel_deg: 20.0
+    maximum_return_error_deg: 1.0
+    # Zero direction is always T3 centre -> fitted circle centre. T3's printed
+    # orientation and corner +x direction are deliberately not used.
+    maximum_zero_radial_error_px: 4.0
+    # Detect a long physical table/reference edge in the lower image. The red
+    # target and blue detected line are display-only aids for manual alignment;
+    # they never block preflight or the start service.
+    camera_alignment_enabled: true
+    camera_alignment_reference_y_ratio: 0.90
+    camera_alignment_roi_y_min_ratio: 0.55
+    camera_alignment_roi_y_max_ratio: 0.98
+    camera_alignment_minimum_line_length_ratio: 0.30
+    camera_alignment_max_candidate_angle_deg: 15.0
+    camera_alignment_max_angle_deg: 0.5
+    camera_alignment_max_vertical_offset_px: 12.0
+    camera_alignment_required_frames: 10
+    camera_alignment_minimum_detection_rate: 0.8
+    camera_alignment_max_age_seconds: 1.0
+    publish_debug_image: true
+    debug_max_rate_hz: 10.0
+    debug_scale: 0.75

src/g20_thumb_apriltag_calibration/config/front_tags.yaml

@@ -0,0 +1,30 @@
+/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    # Live calibration needs the newest frame, not lossless delivery of stale
+    # frames. BEST_EFFORT prevents a slow full-resolution detection callback
+    # from back-pressuring image_proc's reliable image publisher.
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.01
+    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]
+      frames: [tag_t0, tag_t3, tag_t4, tag_t5]
+      sizes: [0.010, 0.010, 0.010, 0.010]
+
+g20_thumb_calibration:
+  ros__parameters:
+    tag_roles: [t0, t3, t4, t5]
+    tag_ids: [0, 1, 2, 3]
+    tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
+    tag_sizes_m: [0.010, 0.010, 0.010, 0.010]

src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py

@@ -0,0 +1,5 @@
+"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
+
+from .core import BASELINE_COMMAND, COMMAND_NAMES
+
+__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py

@@ -0,0 +1,681 @@
+"""Hardware-independent point acquisition state."""
+
+from __future__ import annotations
+
+from bisect import bisect_left
+from collections import deque
+from dataclasses import dataclass, field
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .core import PAIR_NAMES, robust_rotation_summary
+from .pnp import SquareTagPose
+
+
+TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
+    "t0_t3": ("t0", "t3"),
+    "t3_t4": ("t3", "t4"),
+    "t4_t5": ("t4", "t5"),
+}
+
+
+@dataclass(frozen=True)
+class TagQuality:
+    hamming: int
+    decision_margin: float
+    edge_pixels: float
+    reprojection_error_px: float | None = None
+
+
+def tag_quality_is_valid(
+    quality: TagQuality,
+    *,
+    maximum_hamming: int,
+    minimum_decision_margin: float,
+    minimum_edge_pixels: float,
+    maximum_reprojection_error_px: float | None = None,
+) -> bool:
+    detection_valid = (
+        quality.hamming <= maximum_hamming
+        and quality.decision_margin >= minimum_decision_margin
+        and quality.edge_pixels >= minimum_edge_pixels
+    )
+    if not detection_valid:
+        return False
+    if maximum_reprojection_error_px is None:
+        return True
+    return (
+        quality.reprojection_error_px is not None
+        and quality.reprojection_error_px <= maximum_reprojection_error_px
+    )
+
+
+@dataclass(frozen=True)
+class Observation:
+    stamp_ns: int
+    received_at: float
+    relative_quaternion_xyzw: Mapping[str, tuple[float, float, float, float]]
+    tag_quality: Mapping[str, TagQuality]
+    state_u8: tuple[float, ...] = ()
+    state_stamp_ns: int | None = None
+    state_sync_error_ns: int | None = None
+    tag_quaternion_xyzw: Mapping[
+        str, tuple[float, float, float, float]
+    ] = field(default_factory=dict)
+    tag_translation_xyz_m: Mapping[
+        str, tuple[float, float, float]
+    ] = field(default_factory=dict)
+    tag_pose_candidates: Mapping[
+        str, tuple[SquareTagPose, ...]
+    ] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class StateSample:
+    stamp_ns: int
+    position_u8: tuple[float, ...]
+
+
+def interpolate_state_u8(
+    samples: Sequence[StateSample],
+    stamp_ns: int,
+    *,
+    maximum_skew_ns: int,
+) -> tuple[tuple[float, ...], int] | None:
+    """Interpolate the 20-D hand state at an image timestamp.
+
+    The SDK publishes state independently from the camera.  Continuous
+    calibration must therefore use the image timestamp instead of whichever
+    state happened to arrive most recently in the ROS callback thread.
+    """
+    if maximum_skew_ns < 0:
+        raise ValueError("maximum_skew_ns must be non-negative")
+    if not samples:
+        return None
+    stamps = [int(sample.stamp_ns) for sample in samples]
+    index = bisect_left(stamps, int(stamp_ns))
+
+    if index < len(samples) and stamps[index] == int(stamp_ns):
+        state = samples[index].position_u8
+        return (tuple(float(value) for value in state), 0)
+
+    before = samples[index - 1] if index > 0 else None
+    after = samples[index] if index < len(samples) else None
+    if before is not None and after is not None:
+        before_gap = int(stamp_ns) - int(before.stamp_ns)
+        after_gap = int(after.stamp_ns) - int(stamp_ns)
+        nearest_gap = min(before_gap, after_gap)
+        if nearest_gap > maximum_skew_ns:
+            return None
+        denominator = int(after.stamp_ns) - int(before.stamp_ns)
+        if denominator <= 0:
+            return (
+                tuple(float(value) for value in before.position_u8),
+                nearest_gap,
+            )
+        fraction = before_gap / denominator
+        before_values = np.asarray(before.position_u8, dtype=float)
+        after_values = np.asarray(after.position_u8, dtype=float)
+        if before_values.shape != (20,) or after_values.shape != (20,):
+            return None
+        interpolated = before_values + fraction * (after_values - before_values)
+        return (
+            tuple(float(value) for value in interpolated),
+            nearest_gap,
+        )
+
+    nearest = before if before is not None else after
+    if nearest is None:
+        return None
+    gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
+    if gap > maximum_skew_ns or len(nearest.position_u8) != 20:
+        return None
+    return (tuple(float(value) for value in nearest.position_u8), gap)
+
+
+class ContinuousSweepCollector:
+    """Collect timestamp-synchronised observations during one end-to-end move."""
+
+    def __init__(
+        self,
+        *,
+        endpoint_tolerance_u8: float = 2.0,
+        endpoint_hold_seconds: float = 1.0,
+        timeout_seconds: float = 90.0,
+        invalid_timeout_seconds: float = 2.0,
+        minimum_valid_frames: int = 40,
+        minimum_state_span_u8: float = 240.0,
+    ) -> None:
+        if endpoint_tolerance_u8 < 0.0:
+            raise ValueError("endpoint_tolerance_u8 must be non-negative")
+        if endpoint_hold_seconds <= 0.0:
+            raise ValueError("endpoint_hold_seconds must be positive")
+        if timeout_seconds <= 0.0 or invalid_timeout_seconds <= 0.0:
+            raise ValueError("sweep timeouts must be positive")
+        if minimum_valid_frames < 3:
+            raise ValueError("minimum_valid_frames must be at least 3")
+        if minimum_state_span_u8 <= 0.0:
+            raise ValueError("minimum_state_span_u8 must be positive")
+        self.endpoint_tolerance_u8 = float(endpoint_tolerance_u8)
+        self.endpoint_hold_seconds = float(endpoint_hold_seconds)
+        self.timeout_seconds = float(timeout_seconds)
+        self.invalid_timeout_seconds = float(invalid_timeout_seconds)
+        self.minimum_valid_frames = int(minimum_valid_frames)
+        self.minimum_state_span_u8 = float(minimum_state_span_u8)
+        self.observations: list[Observation] = []
+        self.motor_index = 0
+        self.start_u8 = 255.0
+        self.target_u8 = 0.0
+        self.started_at: float | None = None
+        self.last_valid_at: float | None = None
+        self.endpoint_since: float | None = None
+        self.state = "idle"
+        self.reason = ""
+
+    def start(
+        self,
+        now: float,
+        *,
+        motor_index: int,
+        start_u8: int,
+        target_u8: int,
+    ) -> None:
+        if motor_index not in (0, 15):
+            raise ValueError("continuous thumb sweep only permits motor 0 or 15")
+        if {int(start_u8), int(target_u8)} != {0, 255}:
+            raise ValueError("continuous sweep endpoints must be 0 and 255")
+        self.observations.clear()
+        self.motor_index = int(motor_index)
+        self.start_u8 = float(start_u8)
+        self.target_u8 = float(target_u8)
+        self.started_at = float(now)
+        self.last_valid_at = float(now)
+        self.endpoint_since = None
+        self.state = "collecting"
+        self.reason = ""
+
+    @property
+    def active(self) -> bool:
+        return self.state == "collecting"
+
+    @property
+    def valid_frames_seen(self) -> int:
+        return len(self.observations)
+
+    @property
+    def state_span_u8(self) -> float:
+        if not self.observations:
+            return 0.0
+        values = [
+            float(observation.state_u8[self.motor_index])
+            for observation in self.observations
+        ]
+        return float(max(values) - min(values))
+
+    def add(
+        self, observation: Observation, now: float
+    ) -> list[Observation] | None:
+        if not self.active:
+            return None
+        if (
+            len(observation.state_u8) != 20
+            or observation.state_sync_error_ns is None
+        ):
+            return None
+        value = float(observation.state_u8[self.motor_index])
+        if not np.isfinite(value) or not -3.0 <= value <= 258.0:
+            return None
+        now = float(now)
+        self.observations.append(observation)
+        self.last_valid_at = now
+
+        if abs(value - self.target_u8) <= self.endpoint_tolerance_u8:
+            if self.endpoint_since is None:
+                self.endpoint_since = now
+        else:
+            self.endpoint_since = None
+
+        enough_endpoint_hold = (
+            self.endpoint_since is not None
+            and now - self.endpoint_since >= self.endpoint_hold_seconds
+        )
+        if (
+            enough_endpoint_hold
+            and len(self.observations) >= self.minimum_valid_frames
+            and self.state_span_u8 >= self.minimum_state_span_u8
+        ):
+            self.state = "complete"
+            return list(self.observations)
+        return None
+
+    def poll(self, now: float) -> None:
+        if not self.active:
+            return
+        now = float(now)
+        if now - float(self.started_at) > self.timeout_seconds:
+            self.state = "failed"
+            self.reason = "sweep_timeout"
+        elif now - float(self.last_valid_at) > self.invalid_timeout_seconds:
+            self.state = "failed"
+            self.reason = "synchronised_tag_state_timeout"
+
+
+def aggregate_sweep_observations(
+    observations: Sequence[Observation],
+    *,
+    motor_index: int,
+    start_u8: int,
+    target_u8: int,
+    endpoint_tolerance_u8: float,
+) -> dict[int, dict[str, Any]]:
+    """Robustly aggregate continuous observations into integer motor bins."""
+    if not observations:
+        raise ValueError("cannot aggregate an empty continuous sweep")
+    bins: dict[int, list[Observation]] = {}
+    for observation in observations:
+        if len(observation.state_u8) != 20:
+            continue
+        value = float(observation.state_u8[motor_index])
+        if abs(value - float(start_u8)) <= endpoint_tolerance_u8:
+            command = int(start_u8)
+        elif abs(value - float(target_u8)) <= endpoint_tolerance_u8:
+            command = int(target_u8)
+        else:
+            command = int(np.clip(np.rint(value), 0, 255))
+        bins.setdefault(command, []).append(observation)
+    return {
+        command: aggregate_observations(values)
+        for command, values in sorted(bins.items())
+    }
+
+
+class PointCollector:
+    """Wait for a stable pose, then aggregate a fixed number of frames."""
+
+    def __init__(
+        self,
+        *,
+        stable_frames: int = 15,
+        capture_frames: int = 30,
+        minimum_settle_seconds: float = 0.4,
+        maximum_stable_spread_rad: float = np.deg2rad(0.3),
+        stability_mode: str = "rotation",
+        maximum_stable_translation_spread_m: float = 0.003,
+        settle_timeout_seconds: float = 5.0,
+        capture_timeout_seconds: float = 5.0,
+    ) -> None:
+        if stable_frames < 3 or capture_frames < 3:
+            raise ValueError("stable_frames and capture_frames must be at least 3")
+        self.stable_frames = int(stable_frames)
+        self.capture_frames = int(capture_frames)
+        self.minimum_settle_seconds = float(minimum_settle_seconds)
+        self.maximum_stable_spread_rad = float(maximum_stable_spread_rad)
+        self.stability_mode = str(stability_mode)
+        self.maximum_stable_translation_spread_m = float(
+            maximum_stable_translation_spread_m
+        )
+        if self.stability_mode not in {"rotation", "translation"}:
+            raise ValueError(
+                "stability_mode must be rotation or translation"
+            )
+        if self.maximum_stable_translation_spread_m <= 0.0:
+            raise ValueError(
+                "maximum_stable_translation_spread_m must be positive"
+            )
+        self.settle_timeout_seconds = float(settle_timeout_seconds)
+        self.capture_timeout_seconds = float(capture_timeout_seconds)
+        self._stable: deque[Observation] = deque(maxlen=self.stable_frames)
+        self._captured: list[Observation] = []
+        self._consecutive_invalid_frames = 0
+        self.started_at: float | None = None
+        self.capture_started_at: float | None = None
+        self.state = "idle"
+        self.reason = ""
+        self.stable_spread_rad: dict[str, float] = {}
+        self.stable_spread_m: dict[str, float] = {}
+        self.required_state_index: int | None = None
+        self.required_state_u8: float | None = None
+        self.maximum_state_error_u8: float | None = None
+
+    def start(
+        self,
+        now: float,
+        *,
+        required_state_index: int | None = None,
+        required_state_u8: float | None = None,
+        maximum_state_error_u8: float | None = None,
+    ) -> None:
+        state_constraints = (
+            required_state_index,
+            required_state_u8,
+            maximum_state_error_u8,
+        )
+        if any(value is not None for value in state_constraints) and not all(
+            value is not None for value in state_constraints
+        ):
+            raise ValueError(
+                "point state constraint parameters must be provided together"
+            )
+        if required_state_index is not None:
+            if not 0 <= int(required_state_index) < 20:
+                raise ValueError("required_state_index must be in [0, 19]")
+            if not np.isfinite(float(required_state_u8)):
+                raise ValueError("required_state_u8 must be finite")
+            if float(maximum_state_error_u8) < 0.0:
+                raise ValueError(
+                    "maximum_state_error_u8 must be non-negative"
+                )
+        self._stable.clear()
+        self._captured.clear()
+        self._consecutive_invalid_frames = 0
+        self.started_at = float(now)
+        self.capture_started_at = None
+        self.state = "settling"
+        self.reason = ""
+        self.stable_spread_rad = {}
+        self.stable_spread_m = {}
+        self.required_state_index = (
+            None
+            if required_state_index is None
+            else int(required_state_index)
+        )
+        self.required_state_u8 = (
+            None if required_state_u8 is None else float(required_state_u8)
+        )
+        self.maximum_state_error_u8 = (
+            None
+            if maximum_state_error_u8 is None
+            else float(maximum_state_error_u8)
+        )
+
+    @property
+    def active(self) -> bool:
+        return self.state in {"settling", "capturing"}
+
+    @property
+    def stable_frames_seen(self) -> int:
+        return len(self._stable)
+
+    @property
+    def capture_frames_seen(self) -> int:
+        return len(self._captured)
+
+    def _window_is_stable(self) -> bool:
+        if len(self._stable) < self.stable_frames:
+            return False
+        if self.stability_mode == "translation":
+            spreads: dict[str, float] = {}
+            for pair, (parent, child) in TAG_PAIR_ROLES.items():
+                if any(
+                    parent not in observation.tag_translation_xyz_m
+                    or child not in observation.tag_translation_xyz_m
+                    for observation in self._stable
+                ):
+                    self.reason = f"{pair}_translation_missing"
+                    return False
+                vectors = np.asarray(
+                    [
+                        np.asarray(
+                            observation.tag_translation_xyz_m[child],
+                            dtype=float,
+                        )
+                        - np.asarray(
+                            observation.tag_translation_xyz_m[parent],
+                            dtype=float,
+                        )
+                        for observation in self._stable
+                    ],
+                    dtype=float,
+                )
+                reference = np.median(vectors, axis=0)
+                spreads[pair] = float(
+                    np.max(np.linalg.norm(vectors - reference, axis=1))
+                )
+            self.stable_spread_m = spreads
+            for pair, spread in spreads.items():
+                if spread > self.maximum_stable_translation_spread_m:
+                    self.reason = f"{pair}_not_stable"
+                    return False
+            return True
+        spreads: dict[str, float] = {}
+        for pair in PAIR_NAMES:
+            quaternions = [
+                observation.relative_quaternion_xyzw[pair]
+                for observation in self._stable
+            ]
+            _, spread = robust_rotation_summary(quaternions)
+            spreads[pair] = float(spread)
+        self.stable_spread_rad = spreads
+        for pair, spread in spreads.items():
+            if spread > self.maximum_stable_spread_rad:
+                self.reason = f"{pair}_not_stable"
+                return False
+        return True
+
+    def _state_is_acceptable(self, observation: Observation) -> bool:
+        if self.required_state_index is None:
+            return True
+        if (
+            len(observation.state_u8) != 20
+            or observation.state_sync_error_ns is None
+        ):
+            return False
+        value = float(observation.state_u8[self.required_state_index])
+        return bool(
+            np.isfinite(value)
+            and abs(value - float(self.required_state_u8))
+            <= float(self.maximum_state_error_u8)
+        )
+
+    def _return_to_settling(self, reason: str) -> None:
+        self._stable.clear()
+        self._captured.clear()
+        self._consecutive_invalid_frames = 0
+        self.capture_started_at = None
+        self.state = "settling"
+        self.reason = str(reason)
+        self.stable_spread_rad = {}
+        self.stable_spread_m = {}
+
+    def add(
+        self, observation: Observation, now: float
+    ) -> dict[str, Any] | None:
+        if not self.active:
+            return None
+        self._consecutive_invalid_frames = 0
+        now = float(now)
+        if not self._state_is_acceptable(observation):
+            self._return_to_settling("motor_position_out_of_tolerance")
+            return None
+        if self.state == "settling":
+            self._stable.append(observation)
+            elapsed = now - float(self.started_at)
+            if elapsed >= self.minimum_settle_seconds and self._window_is_stable():
+                self.state = "capturing"
+                self.capture_started_at = now
+                self._captured.clear()
+                self.reason = ""
+            return None
+
+        self._captured.append(observation)
+        if len(self._captured) < self.capture_frames:
+            return None
+        # Validate continuity across the boundary as well as inside the
+        # capture block. A planar branch can switch immediately after the
+        # stable window and then look perfectly stable for every capture
+        # frame; checking only the captured frames would accept that jump.
+        stability_aggregate = aggregate_observations(
+            [*self._stable, *self._captured]
+        )
+        if self.stability_mode == "translation":
+            unstable_pairs = [
+                pair
+                for pair, spread in stability_aggregate[
+                    "maximum_translation_spread_m"
+                ].items()
+                if float(spread)
+                > self.maximum_stable_translation_spread_m
+            ]
+        else:
+            unstable_pairs = [
+                pair
+                for pair, spread in stability_aggregate[
+                    "maximum_spread_rad"
+                ].items()
+                if float(spread) > self.maximum_stable_spread_rad
+            ]
+        if unstable_pairs:
+            self._return_to_settling(
+                f"{unstable_pairs[0]}_capture_not_stable"
+            )
+            return None
+        self.state = "complete"
+        return aggregate_observations(self._captured)
+
+    def poll(self, now: float) -> None:
+        if not self.active:
+            return
+        now = float(now)
+        if self.state == "settling":
+            if now - float(self.started_at) > self.settle_timeout_seconds:
+                self.state = "failed"
+                self.reason = self.reason or "settle_timeout"
+        elif self.state == "capturing":
+            if now - float(self.capture_started_at) > self.capture_timeout_seconds:
+                self.state = "failed"
+                self.reason = "capture_timeout"
+
+    def mark_invalid_frame(self) -> None:
+        """Skip one invalid frame while retaining the recent valid window."""
+        if self.state == "capturing":
+            self._return_to_settling("invalid_tag_frame")
+            return
+        if self.state == "settling":
+            self._consecutive_invalid_frames += 1
+            if self._consecutive_invalid_frames >= 3:
+                self._stable.clear()
+            self.reason = "invalid_tag_frame"
+
+
+def aggregate_observations(
+    observations: Sequence[Observation],
+) -> dict[str, Any]:
+    if not observations:
+        raise ValueError("cannot aggregate an empty observation sequence")
+    relative: dict[str, list[float]] = {}
+    spread: dict[str, float] = {}
+    for pair in PAIR_NAMES:
+        quaternion, maximum = robust_rotation_summary(
+            [
+                observation.relative_quaternion_xyzw[pair]
+                for observation in observations
+            ]
+        )
+        relative[pair] = [float(value) for value in quaternion]
+        spread[pair] = float(maximum)
+
+    quality: dict[str, dict[str, float]] = {}
+    tag_names = sorted(observations[0].tag_quality)
+    for tag_name in tag_names:
+        values = [
+            observation.tag_quality[tag_name] for observation in observations
+        ]
+        quality[tag_name] = {
+            "minimum_decision_margin": float(
+                min(value.decision_margin for value in values)
+            ),
+            "minimum_edge_pixels": float(min(value.edge_pixels for value in values)),
+            "maximum_hamming": int(max(value.hamming for value in values)),
+        }
+        reprojection_errors = [
+            float(value.reprojection_error_px)
+            for value in values
+            if value.reprojection_error_px is not None
+        ]
+        if reprojection_errors:
+            quality[tag_name]["maximum_reprojection_error_px"] = float(
+                max(reprojection_errors)
+            )
+
+    states = [
+        observation.state_u8
+        for observation in observations
+        if len(observation.state_u8) == 20
+    ]
+    state_median: list[float] = []
+    if states:
+        state_median = [
+            float(value)
+            for value in np.median(np.asarray(states, dtype=float), axis=0)
+        ]
+    sync_errors = [
+        int(observation.state_sync_error_ns)
+        for observation in observations
+        if observation.state_sync_error_ns is not None
+    ]
+    tag_translations: dict[str, list[float]] = {}
+    translation_spread: dict[str, float] = {}
+    translation_roles = sorted(
+        set.intersection(
+            *(
+                set(observation.tag_translation_xyz_m)
+                for observation in observations
+            )
+        )
+        if observations
+        else set()
+    )
+    for role in translation_roles:
+        values = np.asarray(
+            [
+                observation.tag_translation_xyz_m[role]
+                for observation in observations
+            ],
+            dtype=float,
+        )
+        if values.shape == (len(observations), 3) and np.all(
+            np.isfinite(values)
+        ):
+            tag_translations[role] = [
+                float(value)
+                for value in np.median(values, axis=0)
+            ]
+    for pair, (parent, child) in TAG_PAIR_ROLES.items():
+        if parent not in translation_roles or child not in translation_roles:
+            continue
+        vectors = np.asarray(
+            [
+                np.asarray(
+                    observation.tag_translation_xyz_m[child],
+                    dtype=float,
+                )
+                - np.asarray(
+                    observation.tag_translation_xyz_m[parent],
+                    dtype=float,
+                )
+                for observation in observations
+            ],
+            dtype=float,
+        )
+        reference = np.median(vectors, axis=0)
+        translation_spread[pair] = float(
+            np.max(np.linalg.norm(vectors - reference, axis=1))
+        )
+
+    return {
+        "stamp_start_ns": int(observations[0].stamp_ns),
+        "stamp_end_ns": int(observations[-1].stamp_ns),
+        "valid_frames": len(observations),
+        "relative_quaternion_xyzw": relative,
+        "maximum_spread_rad": spread,
+        "tag_quality": quality,
+        "state_u8_median": state_median,
+        "tag_translation_xyz_m": tag_translations,
+        "maximum_translation_spread_m": translation_spread,
+        "maximum_state_sync_error_ms": (
+            None
+            if not sync_errors
+            else float(max(sync_errors)) / 1_000_000.0
+        ),
+    }

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py

@@ -0,0 +1,638 @@
+"""Pure calibration math and command helpers.
+
+This module deliberately has no ROS imports so the geometry, fitting, and
+output schema can be tested without a camera or a connected hand.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import math
+from typing import Any, Iterable, Mapping, Sequence
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+BASELINE_COMMAND: tuple[int, ...] = (
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    193,
+    148,
+    105,
+    42,
+    245,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+)
+
+PAIR_ROOT = "t0_t3"
+PAIR_MCP = "t3_t4"
+PAIR_IP = "t4_t5"
+PAIR_NAMES: tuple[str, ...] = (PAIR_ROOT, PAIR_MCP, PAIR_IP)
+
+DIRECTION_DECREASING = "decreasing"
+DIRECTION_INCREASING = "increasing"
+DIRECTIONS: tuple[str, ...] = (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+)
+
+PHASE_ROOT = "root"
+PHASE_TIP = "tip"
+
+JOINT_SPECS: dict[str, tuple[str, str, int]] = {
+    "thumb_cmc_pitch": (PHASE_ROOT, PAIR_ROOT, 0),
+    "thumb_mcp": (PHASE_TIP, PAIR_MCP, 15),
+    "thumb_ip": (PHASE_TIP, PAIR_IP, 15),
+}
+
+
+def build_command(
+    motor_index: int,
+    command_u8: int,
+    baseline: Sequence[int] = BASELINE_COMMAND,
+) -> list[int]:
+    """Return one full G20 command with exactly one replaced motor slot."""
+    if len(baseline) != 20:
+        raise ValueError("baseline must contain exactly 20 values")
+    values = [int(value) for value in baseline]
+    if any(value < 0 or value > 255 for value in values):
+        raise ValueError("baseline values must be in [0, 255]")
+    if motor_index not in (0, 5, 15):
+        raise ValueError(
+            "front thumb calibration only permits motor 0, 5 or 15"
+        )
+    command_u8 = int(command_u8)
+    if command_u8 < 0 or command_u8 > 255:
+        raise ValueError("command_u8 must be in [0, 255]")
+    values[motor_index] = command_u8
+    return values
+
+
+def scan_targets(
+    repetitions: int = 3,
+    command_step: int = 1,
+) -> list[tuple[int, str, int]]:
+    """Build repeated 255->0->255 scan targets on a bounded command grid."""
+    if repetitions < 1:
+        raise ValueError("repetitions must be positive")
+    if command_step < 1 or command_step > 255:
+        raise ValueError("command_step must be in [1, 255]")
+    increasing = list(range(0, 256, command_step))
+    if increasing[-1] != 255:
+        increasing.append(255)
+    decreasing = list(reversed(increasing))
+    targets: list[tuple[int, str, int]] = []
+    for cycle in range(repetitions):
+        targets.extend(
+            (cycle, DIRECTION_DECREASING, command)
+            for command in decreasing
+        )
+        targets.extend(
+            (cycle, DIRECTION_INCREASING, command)
+            for command in increasing
+        )
+    return targets
+
+
+def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
+    quaternion = np.asarray(values, dtype=float)
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion must contain four finite xyzw values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1e-12:
+        raise ValueError("quaternion norm is zero")
+    return quaternion / norm
+
+
+def relative_quaternion_xyzw(
+    parent_camera_quaternion: Sequence[float],
+    child_camera_quaternion: Sequence[float],
+) -> tuple[float, float, float, float]:
+    """Compute parent->child orientation from two camera->tag rotations."""
+    parent = Rotation.from_quat(normalize_quaternion_xyzw(parent_camera_quaternion))
+    child = Rotation.from_quat(normalize_quaternion_xyzw(child_camera_quaternion))
+    quaternion = (parent.inv() * child).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def image_plane_tag_quaternion_xyzw(
+    corners_xy: Sequence[Sequence[float]],
+) -> tuple[float, float, float, float]:
+    """Estimate tag orientation about the optical axis from ordered corners."""
+    corners = np.asarray(corners_xy, dtype=float)
+    if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+        raise ValueError("corners_xy must contain four finite xy points")
+    # AprilTag corners 0->1 and 3->2 both follow the tag-local x axis.
+    # Average the two edges to reduce sub-pixel corner noise and perspective
+    # asymmetry. Image y points down, hence the minus sign for a right-handed
+    # camera-frame z rotation.
+    x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
+    if float(np.linalg.norm(x_axis)) < 1e-9:
+        raise ValueError("tag x-axis is degenerate")
+    angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
+    quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def robust_rotation_summary(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> tuple[tuple[float, float, float, float], float]:
+    """Return a robust orientation and maximum angular residual in radians."""
+    if not quaternions_xyzw:
+        raise ValueError("at least one quaternion is required")
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    reference = rotations[0]
+    delta_vectors = (reference.inv() * rotations).as_rotvec()
+    median_delta = np.median(delta_vectors, axis=0)
+    robust = reference * Rotation.from_rotvec(median_delta)
+    residuals = (robust.inv() * rotations).magnitude()
+    maximum = float(np.max(residuals)) if residuals.size else 0.0
+    return (
+        tuple(float(value) for value in robust.as_quat()),
+        maximum,
+    )
+
+
+def rotation_spread_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> float:
+    """Return the maximum geodesic residual around a robust orientation."""
+    _, spread = robust_rotation_summary(quaternions_xyzw)
+    return spread
+
+
+def rotation_rms_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float | None = None,
+) -> float:
+    """Return RMS geodesic noise around a robust orientation."""
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    if outlier_threshold_rad is not None:
+        threshold = float(outlier_threshold_rad)
+        if threshold <= 0.0:
+            raise ValueError("outlier_threshold_rad must be positive")
+        residuals = residuals[residuals <= threshold]
+        if residuals.size == 0:
+            return float("inf")
+    return float(np.sqrt(np.mean(np.square(residuals))))
+
+
+def rotation_inlier_fraction(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float,
+) -> float:
+    """Return the fraction close to the robust orientation."""
+    threshold = float(outlier_threshold_rad)
+    if threshold <= 0.0:
+        raise ValueError("outlier_threshold_rad must be positive")
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    return float(np.mean(residuals <= threshold))
+
+
+def delta_rotation_vector(
+    reference_xyzw: Sequence[float],
+    observed_xyzw: Sequence[float],
+) -> np.ndarray:
+    reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
+    observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
+    return (reference.inv() * observed).as_rotvec()
+
+
+def fit_rotation_axis(
+    vectors: Sequence[Sequence[float]],
+    commands: Sequence[int],
+) -> np.ndarray:
+    """Fit and orient the single rotational axis used by one motor sweep."""
+    matrix = np.asarray(vectors, dtype=float)
+    command_values = np.asarray(commands, dtype=int)
+    if matrix.ndim != 2 or matrix.shape[1] != 3:
+        raise ValueError("vectors must have shape (N, 3)")
+    if command_values.shape != (matrix.shape[0],):
+        raise ValueError("commands must match vectors")
+    useful = np.linalg.norm(matrix, axis=1) > 1e-6
+    if int(np.count_nonzero(useful)) < 3:
+        raise ValueError("insufficient non-zero rotations to fit an axis")
+    _, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
+    axis = vh[0]
+    projections = matrix @ axis
+    low = projections[command_values <= 16]
+    high = projections[command_values >= 239]
+    if low.size and high.size and float(np.median(low)) < float(np.median(high)):
+        axis = -axis
+    return axis / np.linalg.norm(axis)
+
+
+def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
+    """Unweighted PAVA projection onto non-increasing values."""
+    original = np.asarray(values, dtype=float)
+    if original.ndim != 1 or not np.all(np.isfinite(original)):
+        raise ValueError("values must be a finite vector")
+    negated = -original
+    levels: list[float] = []
+    weights: list[int] = []
+    starts: list[int] = []
+    for index, value in enumerate(negated):
+        levels.append(float(value))
+        weights.append(1)
+        starts.append(index)
+        while len(levels) >= 2 and levels[-2] > levels[-1]:
+            total_weight = weights[-2] + weights[-1]
+            merged = (
+                levels[-2] * weights[-2] + levels[-1] * weights[-1]
+            ) / total_weight
+            levels[-2:] = [merged]
+            weights[-2:] = [total_weight]
+            starts.pop()
+    projected = np.empty_like(original)
+    for block_index, (level, start) in enumerate(zip(levels, starts)):
+        end = starts[block_index + 1] if block_index + 1 < len(starts) else len(original)
+        projected[start:end] = -level
+    return projected
+
+
+def _record_rotation(record: Mapping[str, Any], pair: str) -> tuple[float, ...]:
+    rotations = record.get("relative_quaternion_xyzw", {})
+    value = rotations.get(pair)
+    if value is None:
+        raise ValueError(f"sample record is missing {pair}")
+    return tuple(float(component) for component in value)
+
+
+@dataclass(frozen=True)
+class FitResult:
+    joints: dict[str, dict[str, Any]]
+    axes: dict[str, tuple[float, float, float]]
+    references: dict[str, tuple[float, float, float, float]]
+    ip_coupling: dict[str, float]
+    max_monotonic_correction_rad: float
+    max_hysteresis_rad: float
+    measurement_mode: str = "rotation"
+    trajectory_models: dict[str, Any] = field(default_factory=dict)
+    trajectory_quality: dict[str, Any] = field(default_factory=dict)
+
+    def measure_from_reference(
+        self,
+        joint_name: str,
+        observed_quaternion_xyzw: Sequence[float],
+        reference_quaternion_xyzw: Sequence[float] | None = None,
+    ) -> float:
+        reference = (
+            reference_quaternion_xyzw
+            if reference_quaternion_xyzw is not None
+            else self.references[joint_name]
+        )
+        vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
+        axis = np.asarray(self.axes[joint_name], dtype=float)
+        return float(vector @ axis)
+
+
+def fit_calibration_curves(records: Iterable[Mapping[str, Any]]) -> FitResult:
+    """Fit six complete 256-entry curves from dense or sparse scan records."""
+    samples = [
+        dict(record)
+        for record in records
+        if record.get("kind", "sample") == "sample"
+    ]
+    if not samples:
+        raise ValueError("no scan records were provided")
+
+    joint_results: dict[str, dict[str, Any]] = {}
+    axes: dict[str, tuple[float, float, float]] = {}
+    references: dict[str, tuple[float, float, float, float]] = {}
+    maximum_correction = 0.0
+    maximum_hysteresis = 0.0
+
+    for joint_name, (phase, pair, motor_index) in JOINT_SPECS.items():
+        phase_records = [record for record in samples if record.get("phase") == phase]
+        if not phase_records:
+            raise ValueError(f"no records for phase {phase}")
+
+        cycle_references: dict[int, tuple[float, ...]] = {}
+        for record in phase_records:
+            if (
+                record.get("direction") == DIRECTION_DECREASING
+                and int(record.get("command_u8", -1)) == 255
+            ):
+                cycle_references.setdefault(
+                    int(record["cycle"]),
+                    _record_rotation(record, pair),
+                )
+        cycles = sorted({int(record["cycle"]) for record in phase_records})
+        if any(cycle not in cycle_references for cycle in cycles):
+            raise ValueError(f"{joint_name} is missing a command-255 cycle reference")
+
+        vectors: list[np.ndarray] = []
+        commands: list[int] = []
+        indexed: list[tuple[Mapping[str, Any], np.ndarray]] = []
+        for record in phase_records:
+            cycle = int(record["cycle"])
+            vector = delta_rotation_vector(
+                cycle_references[cycle],
+                _record_rotation(record, pair),
+            )
+            vectors.append(vector)
+            commands.append(int(record["command_u8"]))
+            indexed.append((record, vector))
+        axis = fit_rotation_axis(vectors, commands)
+        axes[joint_name] = tuple(float(value) for value in axis)
+        references[joint_name] = robust_rotation_summary(
+            list(cycle_references.values())
+        )[0]
+
+        branch_values: dict[str, list[list[float]]] = {
+            direction: [[] for _ in range(256)] for direction in DIRECTIONS
+        }
+        for record, vector in indexed:
+            direction = str(record["direction"])
+            command = int(record["command_u8"])
+            branch_values[direction][command].append(float(vector @ axis))
+
+        fitted_branches: dict[str, list[float]] = {}
+        for direction in DIRECTIONS:
+            sample_commands = np.asarray(
+                [
+                    command
+                    for command, values in enumerate(branch_values[direction])
+                    if values
+                ],
+                dtype=int,
+            )
+            if (
+                sample_commands.size < 3
+                or int(sample_commands[0]) != 0
+                or int(sample_commands[-1]) != 255
+            ):
+                raise ValueError(
+                    f"{joint_name}.{direction} requires at least three samples "
+                    "including commands 0 and 255"
+                )
+            raw = np.asarray(
+                [
+                    float(np.median(branch_values[direction][command]))
+                    for command in sample_commands
+                ],
+                dtype=float,
+            )
+            raw -= raw[-1]
+            projected_samples = isotonic_nonincreasing(raw)
+            projected_samples -= projected_samples[-1]
+            correction = float(np.max(np.abs(projected_samples - raw)))
+            maximum_correction = max(maximum_correction, correction)
+            projected = np.interp(
+                np.arange(256, dtype=float),
+                sample_commands.astype(float),
+                projected_samples,
+            )
+            projected -= projected[255]
+            fitted_branches[direction] = [
+                round(float(value), 8) for value in projected
+            ]
+
+        hysteresis = float(
+            np.max(
+                np.abs(
+                    np.asarray(fitted_branches[DIRECTION_DECREASING])
+                    - np.asarray(fitted_branches[DIRECTION_INCREASING])
+                )
+            )
+        )
+        maximum_hysteresis = max(maximum_hysteresis, hysteresis)
+        combined_curve = 0.5 * (
+            np.asarray(
+                fitted_branches[DIRECTION_DECREASING], dtype=float
+            )
+            + np.asarray(
+                fitted_branches[DIRECTION_INCREASING], dtype=float
+            )
+        )
+        combined_curve -= combined_curve[255]
+        joint_result: dict[str, Any] = {
+            "motor_index": motor_index,
+            "angle_rad": [
+                round(float(value), 8) for value in combined_curve
+            ],
+            "decreasing_rad": fitted_branches[DIRECTION_DECREASING],
+            "increasing_rad": fitted_branches[DIRECTION_INCREASING],
+        }
+        if joint_name == "thumb_ip":
+            joint_result["passive"] = True
+        joint_results[joint_name] = joint_result
+
+    mcp = joint_results["thumb_mcp"]
+    ip = joint_results["thumb_ip"]
+    x = np.asarray(mcp["angle_rad"], dtype=float)
+    y = np.asarray(ip["angle_rad"], dtype=float)
+    design = np.column_stack((x, np.ones_like(x)))
+    multiplier, offset = np.linalg.lstsq(design, y, rcond=None)[0]
+    predicted = multiplier * x + offset
+    residual_sum = float(np.sum((y - predicted) ** 2))
+    total_sum = float(np.sum((y - np.mean(y)) ** 2))
+    r_squared = 1.0 if total_sum < 1e-12 else 1.0 - residual_sum / total_sum
+
+    return FitResult(
+        joints=joint_results,
+        axes=axes,
+        references=references,
+        ip_coupling={
+            "multiplier": round(float(multiplier), 8),
+            "offset_rad": round(float(offset), 8),
+            "r_squared": round(float(r_squared), 8),
+        },
+        max_monotonic_correction_rad=maximum_correction,
+        max_hysteresis_rad=maximum_hysteresis,
+    )
+
+
+def create_final_payload(
+    *,
+    serial_number: str,
+    fit: FitResult,
+    validation_errors_rad: Sequence[float],
+    passed: bool,
+    baseline: Sequence[int] = BASELINE_COMMAND,
+) -> dict[str, Any]:
+    errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
+    mae = float(np.mean(errors)) if errors.size else float("nan")
+    p95 = float(np.percentile(errors, 95)) if errors.size else float("nan")
+    runtime_joints: dict[str, dict[str, Any]] = {}
+    for joint_name, joint in fit.joints.items():
+        runtime_joint: dict[str, Any] = {
+            "motor_index": int(joint["motor_index"]),
+            "angle_rad": [
+                round(float(value), 8) for value in joint["angle_rad"]
+            ],
+        }
+        if joint_name == "thumb_ip":
+            runtime_joint["passive"] = True
+        runtime_joints[joint_name] = runtime_joint
+
+    payload = {
+        "schema_version": 2,
+        "model": "G20",
+        "side": "left",
+        "serial_number": str(serial_number),
+        "angle_unit": "rad",
+        "command_range": [0, 255],
+        "zero_command_u8": 255,
+        "baseline_command_u8": [int(value) for value in baseline],
+        "joints": runtime_joints,
+        "ip_coupling": {
+            "multiplier": fit.ip_coupling["multiplier"],
+            "offset_rad": fit.ip_coupling["offset_rad"],
+        },
+        "quality": {
+            "passed": bool(passed),
+            "validation_mae_rad": None if not np.isfinite(mae) else round(mae, 8),
+            "validation_p95_rad": None if not np.isfinite(p95) else round(p95, 8),
+        },
+    }
+    validate_final_payload(payload)
+    return payload
+
+
+def maximum_non_target_drift_rad(
+    records: Iterable[Mapping[str, Any]],
+    fit: FitResult,
+) -> float:
+    """Measure unintended active-joint motion during the two isolated scans."""
+    samples = [
+        dict(record)
+        for record in records
+        if record.get("kind", "sample") == "sample"
+    ]
+    maximum = 0.0
+    checks = (
+        (PHASE_ROOT, "thumb_mcp", PAIR_MCP),
+        (PHASE_ROOT, "thumb_ip", PAIR_IP),
+        (PHASE_TIP, "thumb_cmc_pitch", PAIR_ROOT),
+    )
+    for phase, joint_name, pair in checks:
+        phase_records = [record for record in samples if record.get("phase") == phase]
+        for cycle in sorted({int(record["cycle"]) for record in phase_records}):
+            cycle_records = [
+                record for record in phase_records if int(record["cycle"]) == cycle
+            ]
+            reference_record = next(
+                (
+                    record
+                    for record in cycle_records
+                    if record.get("direction") == DIRECTION_DECREASING
+                    and int(record.get("command_u8", -1)) == 255
+                ),
+                None,
+            )
+            if reference_record is None:
+                continue
+            reference = _record_rotation(reference_record, pair)
+            axis = np.asarray(fit.axes[joint_name], dtype=float)
+            for record in cycle_records:
+                drift = abs(
+                    float(
+                        delta_rotation_vector(
+                            reference,
+                            _record_rotation(record, pair),
+                        )
+                        @ axis
+                    )
+                )
+                maximum = max(maximum, drift)
+    return maximum
+
+
+def validate_final_payload(payload: Mapping[str, Any]) -> None:
+    """Validate the deliberately small runtime JSON schema."""
+    if payload.get("schema_version") != 2:
+        raise ValueError("schema_version must be 2")
+    if payload.get("model") != "G20" or payload.get("side") != "left":
+        raise ValueError("payload must describe a left G20")
+    if payload.get("angle_unit") != "rad":
+        raise ValueError("angle_unit must be rad")
+    baseline = payload.get("baseline_command_u8")
+    if not isinstance(baseline, list) or len(baseline) != 20:
+        raise ValueError("baseline_command_u8 must contain 20 values")
+    joints = payload.get("joints")
+    if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS):
+        raise ValueError("payload must contain exactly the three thumb joints")
+    for joint_name, joint in joints.items():
+        expected_motor = JOINT_SPECS[joint_name][2]
+        if int(joint.get("motor_index", -1)) != expected_motor:
+            raise ValueError(f"{joint_name} has the wrong motor index")
+        curve = joint.get("angle_rad")
+        if not isinstance(curve, list) or len(curve) != 256:
+            raise ValueError(
+                f"{joint_name}.angle_rad must contain 256 values"
+            )
+        values = np.asarray(curve, dtype=float)
+        if not np.all(np.isfinite(values)):
+            raise ValueError(
+                f"{joint_name}.angle_rad contains non-finite values"
+            )
+        if np.any(np.diff(values) > 1e-7):
+            raise ValueError(
+                f"{joint_name}.angle_rad must be non-increasing"
+            )
+        if abs(float(values[255])) > 1e-6:
+            raise ValueError(
+                f"{joint_name}.angle_rad[255] must be zero"
+            )
+    if joints["thumb_ip"].get("passive") is not True:
+        raise ValueError("thumb_ip must be marked passive")

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py

@@ -0,0 +1,287 @@
+"""Human-readable diagnostics for the calibration status topic."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+from .acquisition import TagQuality
+
+
+ROLE_NAMES_ZH = {
+    "t0": "掌心T0",
+    "t3": "拇指根部T3",
+    "t4": "拇指中节T4",
+    "t5": "拇指末节T5",
+}
+
+STATE_NAMES_ZH = {
+    "PREFLIGHT": "设备和标签预检",
+    "WAIT_ROOT_CONFIRM": "等待开始根部标定",
+    "SCAN_ROOT": "正在标定拇指根部",
+    "WAIT_TIP_CONFIRM": "等待开始尖部标定",
+    "SCAN_TIP": "正在标定拇指尖部",
+    "VALIDATING": "正在随机复测",
+    "PAUSED": "标定已暂停",
+    "ABORTED": "标定已终止",
+    "COMPLETE": "标定已完成",
+}
+
+_REJECTION_ZH = {
+    "camera_info_not_ready": "相机内参尚未就绪",
+    "tag_not_detected": "当前画面未检测到标签",
+    "tag_quality_invalid": "标签图像质量未达到门限",
+    "no_pose_within_reprojection_or_tilt_limit": (
+        "PnP重投影误差过大或标签倾斜角过大"
+    ),
+    "pose_jump": "PnP位置或姿态发生突跳",
+    "pnp_solve_failed": "PnP三维位置求解失败",
+    "group_missing_pose_candidates": "标签组缺少可用的PnP候选解",
+    "group_pose_jump": "标签组PnP结果发生突跳",
+}
+
+
+def pnp_rejection_zh(reason: str) -> str:
+    """Translate a PnP rejection code without hiding its machine value."""
+    return _REJECTION_ZH.get(str(reason), f"PnP拒绝：{reason}")
+
+
+def build_tag_quality_diagnostics(
+    tag_config: Mapping[str, Mapping[str, Any]],
+    qualities: Mapping[str, TagQuality],
+    rejections: Mapping[str, str],
+    reprojection_errors_px: Mapping[str, float],
+    *,
+    maximum_hamming: int,
+    minimum_decision_margin: float,
+    minimum_edge_pixels: float,
+    maximum_reprojection_error_px: float,
+) -> dict[str, dict[str, Any]]:
+    """Return compact per-tag values, failures and Chinese explanations."""
+    result: dict[str, dict[str, Any]] = {}
+    for role, config in tag_config.items():
+        quality = qualities.get(role)
+        rejection = rejections.get(role)
+        problems: list[str] = []
+        if quality is None:
+            problems.append("当前画面未检测到")
+        else:
+            if int(quality.hamming) > int(maximum_hamming):
+                problems.append(
+                    f"hamming={quality.hamming}，要求≤{maximum_hamming}"
+                )
+            if float(quality.decision_margin) < float(
+                minimum_decision_margin
+            ):
+                problems.append(
+                    "识别置信度"
+                    f"{quality.decision_margin:.1f}，要求≥"
+                    f"{minimum_decision_margin:.1f}"
+                )
+            if float(quality.edge_pixels) < float(minimum_edge_pixels):
+                problems.append(
+                    f"边长{quality.edge_pixels:.1f}px，要求≥"
+                    f"{minimum_edge_pixels:.1f}px"
+                )
+        reprojection = reprojection_errors_px.get(role)
+        if (
+            reprojection is not None
+            and float(reprojection) > float(maximum_reprojection_error_px)
+        ):
+            problems.append(
+                f"重投影误差{reprojection:.2f}px，要求≤"
+                f"{maximum_reprojection_error_px:.2f}px"
+            )
+        if rejection is not None and not problems:
+            problems.append(pnp_rejection_zh(rejection))
+
+        detection_valid = bool(
+            quality is not None
+            and int(quality.hamming) <= int(maximum_hamming)
+            and float(quality.decision_margin)
+            >= float(minimum_decision_margin)
+            and float(quality.edge_pixels) >= float(minimum_edge_pixels)
+        )
+        individual_valid = bool(
+            detection_valid
+            and rejection is None
+            and (
+                reprojection is None
+                or float(reprojection)
+                <= float(maximum_reprojection_error_px)
+            )
+        )
+        name_zh = ROLE_NAMES_ZH.get(role, role)
+        result[role] = {
+            "id": int(config["id"]),
+            "name_zh": name_zh,
+            "detected": quality is not None,
+            "individual_valid": individual_valid,
+            "hamming": None if quality is None else int(quality.hamming),
+            "decision_margin": (
+                None
+                if quality is None
+                else round(float(quality.decision_margin), 2)
+            ),
+            "edge_pixels": (
+                None
+                if quality is None
+                else round(float(quality.edge_pixels), 2)
+            ),
+            "reprojection_error_px": (
+                None
+                if reprojection is None
+                else round(float(reprojection), 3)
+            ),
+            "rejection": rejection,
+            "problems_zh": problems,
+            "summary_zh": (
+                f"{name_zh}正常"
+                if not problems
+                else f"{name_zh}：" + "；".join(problems)
+            ),
+        }
+    return result
+
+
+def status_guidance_zh(
+    state: str,
+    reason: str,
+    tag_diagnostics: Mapping[str, Mapping[str, Any]],
+) -> tuple[str, str]:
+    """Return a concise Chinese reason and the next practical action."""
+    reason_text = str(reason)
+    invalid_tags = [
+        item
+        for item in tag_diagnostics.values()
+        if not bool(item.get("individual_valid", False))
+    ]
+    tag_failure = (
+        "；".join(str(item["summary_zh"]) for item in invalid_tags)
+        if invalid_tags
+        else ""
+    )
+    if "invalid_tag_frame" in reason_text or (
+        state == "PREFLIGHT" and invalid_tags
+    ):
+        actions: list[str] = []
+        problem_text = "；".join(
+            str(problem)
+            for item in invalid_tags
+            for problem in item.get("problems_zh", [])
+        )
+        if "未检测到" in problem_text:
+            actions.append("调整遮挡或画面范围，让四张标签同时可见")
+        if "边长" in problem_text:
+            actions.append("将相机稍微靠近，建议标签边长稳定大于32px")
+        if "识别置信度" in problem_text:
+            actions.append("改善照明、对焦并避免标签反光")
+        if "hamming" in problem_text:
+            actions.append("清洁或重新打印标签，并检查对焦")
+        if "相机内参" in problem_text:
+            actions.append("确认CameraInfo话题正常")
+        if not actions:
+            actions.append("保持机械手不动，检查标签和相机画面")
+        actions.append("修正后调用resume继续")
+        return (
+            f"标签帧无效，无法采集。{tag_failure}",
+            "；".join(dict.fromkeys(actions)),
+        )
+
+    exact = {
+        "waiting_for_camera_tags_and_sdk": (
+            "正在等待相机、四张标签和灵巧手SDK就绪",
+            "保持设备连接，观察预检状态",
+        ),
+        "call_start": (
+            "预检已通过，等待开始标定",
+            "调用/g20_thumb_calibration/start",
+        ),
+        "call_confirm_root_full_range": (
+            "预检已通过，等待开始拇指根部全行程标定",
+            "调用/g20_thumb_calibration/start",
+        ),
+        "operator_pause": (
+            "操作员已暂停标定",
+            "确认安全后调用resume继续",
+        ),
+        "calibration_complete": (
+            "标定和随机复测已经完成",
+            "检查最终JSON中的quality.passed",
+        ),
+    }
+    if reason_text in exact:
+        return exact[reason_text]
+    if reason_text.startswith("collecting_detection_preflight"):
+        return "正在采集标签预检帧", "保持相机和机械手静止"
+    if reason_text.startswith("detection_hz_too_low"):
+        return (
+            f"AprilTag检测频率过低（{reason_text.split(':')[-1]}Hz）",
+            "关闭图像显示等额外订阅，检查相机和检测节点负载",
+        )
+    if reason_text.startswith("detection_rate_too_low"):
+        return "四张标签同时有效的比例过低", "检查遮挡、边长和照明"
+    if "not_stable" in reason_text:
+        return "标签三维中心尚未稳定，无法完成静态采集", (
+            "不要触碰相机和机械手，检查标签固定及关节抖动后调用resume"
+        )
+    if reason_text.startswith("continuous_sweep_insufficient_coverage"):
+        return "连续扫描采样覆盖不足", "降低标定速度或提高AprilTag检测频率后重扫"
+    if "synchronised_tag_state_timeout" in reason_text:
+        return (
+            "运动过程中连续3秒没有取得同时有效的标签帧和电机状态；"
+            "暂停后当前画面恢复正常也不会自动继续",
+            "确认四张标签全行程稳定可见，并检查/g20/cb_left_hand_state；"
+            "修正后调用resume继续，不要调用start",
+        )
+    if "timeout" in reason_text:
+        return f"等待超时：{reason_text}", "检查标签、同步状态和机械手是否到位"
+    if reason_text.startswith("pnp_trajectory"):
+        return f"三维轨迹质量检查未通过：{reason_text}", (
+            "检查标签固定、相机稳定和机械抖动"
+        )
+    if reason_text.startswith("root:continuous"):
+        return "正在连续采集拇指根部轨迹", "无需操作，等待该方向完成"
+    if reason_text.startswith("tip:continuous"):
+        return "正在连续采集拇指尖部轨迹", "无需操作，等待该方向完成"
+    return (
+        f"{STATE_NAMES_ZH.get(state, state)}：{reason_text}",
+        "根据英文reason机器码检查详细原因",
+    )
+
+
+def render_status_text_zh(
+    state_zh: str,
+    reason_zh: str,
+    action_zh: str,
+    tag_diagnostics: Mapping[str, Mapping[str, Any]],
+) -> str:
+    """Render a terminal-friendly multiline Chinese status message."""
+    lines = [
+        f"状态：{state_zh}",
+        f"原因：{reason_zh}",
+        f"建议：{action_zh}",
+        "标签：",
+    ]
+    for item in tag_diagnostics.values():
+        detected = bool(item.get("detected", False))
+        valid = bool(item.get("individual_valid", False))
+        if detected:
+            metrics = (
+                f"边长{float(item['edge_pixels']):.1f}px，"
+                f"置信度{float(item['decision_margin']):.1f}，"
+                f"hamming={int(item['hamming'])}"
+            )
+            reprojection = item.get("reprojection_error_px")
+            if reprojection is not None:
+                metrics += f"，重投影{float(reprojection):.2f}px"
+        else:
+            metrics = "未检测到"
+        problems = item.get("problems_zh", [])
+        problem_suffix = (
+            "" if not problems else "；" + "；".join(map(str, problems))
+        )
+        lines.append(
+            f"- {item['name_zh']}(ID {item['id']})："
+            f"{'正常' if valid else '异常'}，{metrics}{problem_suffix}"
+        )
+    return "\n".join(lines)

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py

@@ -0,0 +1,979 @@
+"""Square AprilTag pose estimation with planar ambiguity tracking.
+
+The AprilTag detections contain accurately refined image corners.  This module
+uses OpenCV's IPPE square solver directly so the calibration node can inspect
+both planar PnP solutions instead of accepting an occasionally flipped TF
+pose.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, replace
+from itertools import product
+import math
+from typing import Mapping, Sequence
+
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+@dataclass(frozen=True)
+class SquareTagPose:
+    """One tag-to-camera pose candidate returned by IPPE."""
+
+    quaternion_xyzw: tuple[float, float, float, float]
+    translation_xyz_m: tuple[float, float, float]
+    reprojection_error_px: float
+
+
+def _relative_pose(
+    parent: SquareTagPose,
+    child: SquareTagPose,
+) -> tuple[Rotation, np.ndarray]:
+    parent_rotation = Rotation.from_quat(parent.quaternion_xyzw)
+    child_rotation = Rotation.from_quat(child.quaternion_xyzw)
+    relative_rotation = parent_rotation.inv() * child_rotation
+    relative_translation = parent_rotation.inv().apply(
+        np.asarray(child.translation_xyz_m, dtype=float)
+        - np.asarray(parent.translation_xyz_m, dtype=float)
+    )
+    return relative_rotation, relative_translation
+
+
+def select_rigid_group_trajectory(
+    frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
+    *,
+    roles: Sequence[str],
+    fixed_pairs: Sequence[tuple[str, str]],
+    reprojection_scale_px: float,
+    rotation_scale_rad: float,
+    translation_scale_m: float,
+    pair_geometry: str = "pose",
+) -> tuple[
+    list[dict[str, SquareTagPose]],
+    dict[str, float | str],
+]:
+    """Resolve planar branches using geometry that should stay rigid.
+
+    Every possible branch combination in the first frame is treated as a
+    candidate rigid reference. For each such reference, every later frame
+    independently chooses the combination with the lowest reprojection plus
+    geometric-drift cost. ``pose`` compares relative rotation and translation;
+    ``distance`` compares only Euclidean centre distances and therefore does
+    not allow planar-PnP orientation jitter into centre-trajectory angles.
+    The globally cheapest reference and path win.
+    """
+    role_names = tuple(str(role) for role in roles)
+    pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    if not frames:
+        raise ValueError("at least one PnP frame is required")
+    if len(set(role_names)) != len(role_names) or not role_names:
+        raise ValueError("roles must be non-empty and unique")
+    if any(
+        parent not in role_names or child not in role_names
+        for parent, child in pair_names
+    ):
+        raise ValueError("fixed_pairs must reference roles")
+    reprojection_scale = float(reprojection_scale_px)
+    rotation_scale = float(rotation_scale_rad)
+    translation_scale = float(translation_scale_m)
+    geometry_mode = str(pair_geometry)
+    if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0:
+        raise ValueError("trajectory selection scales must be positive")
+    if geometry_mode not in {"pose", "distance"}:
+        raise ValueError("pair_geometry must be pose or distance")
+
+    combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
+    for frame in frames:
+        candidate_lists = [tuple(frame.get(role, ())) for role in role_names]
+        if any(not candidates for candidates in candidate_lists):
+            raise ValueError("every frame must contain every requested role")
+        combinations_by_frame.append(
+            [
+                dict(zip(role_names, combination))
+                for combination in product(*candidate_lists)
+            ]
+        )
+
+    best_total = float("inf")
+    best_path: list[dict[str, SquareTagPose]] | None = None
+
+    def emission(
+        combination: Mapping[str, SquareTagPose],
+        reference_pairs: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ],
+        reference_distances: Mapping[tuple[str, str], float],
+    ) -> tuple[float, float, float]:
+        reprojection_cost = sum(
+            pose.reprojection_error_px
+            for pose in combination.values()
+        ) / reprojection_scale
+        rotation_drifts: list[float] = []
+        translation_drifts: list[float] = []
+        distance_drifts: list[float] = []
+        for pair, (
+            reference_rotation,
+            reference_translation,
+        ) in reference_pairs.items():
+            rotation, translation = _relative_pose(
+                combination[pair[0]],
+                combination[pair[1]],
+            )
+            rotation_drifts.append(
+                float(
+                    (reference_rotation.inv() * rotation).magnitude()
+                )
+            )
+            translation_drifts.append(
+                float(
+                    np.linalg.norm(
+                        translation - reference_translation
+                    )
+                )
+            )
+            current_distance = float(
+                np.linalg.norm(
+                    np.asarray(
+                        combination[pair[1]].translation_xyz_m,
+                        dtype=float,
+                    )
+                    - np.asarray(
+                        combination[pair[0]].translation_xyz_m,
+                        dtype=float,
+                    )
+                )
+            )
+            distance_drifts.append(
+                abs(current_distance - reference_distances[pair])
+            )
+        if geometry_mode == "distance":
+            geometry_cost = sum(distance_drifts) / translation_scale
+        else:
+            geometry_cost = (
+                sum(rotation_drifts) / rotation_scale
+                + sum(translation_drifts) / translation_scale
+            )
+        return (
+            reprojection_cost + geometry_cost,
+            max(rotation_drifts, default=0.0),
+            (
+                max(distance_drifts, default=0.0)
+                if geometry_mode == "distance"
+                else max(translation_drifts, default=0.0)
+            ),
+        )
+
+    def transition_cost(
+        previous: Mapping[str, SquareTagPose],
+        current: Mapping[str, SquareTagPose],
+    ) -> float:
+        rotation_motion = sum(
+            rotation_distance_rad(
+                previous[role].quaternion_xyzw,
+                current[role].quaternion_xyzw,
+            )
+            for role in role_names
+        )
+        translation_motion = sum(
+            float(
+                np.linalg.norm(
+                    np.asarray(current[role].translation_xyz_m)
+                    - np.asarray(previous[role].translation_xyz_m)
+                )
+            )
+            for role in role_names
+        )
+        if geometry_mode == "distance":
+            return translation_motion / translation_scale
+        return (
+            rotation_motion / rotation_scale
+            + translation_motion / translation_scale
+        )
+
+    for reference_index, reference_combination in enumerate(
+        combinations_by_frame[0]
+    ):
+        reference_pairs = {
+            pair: _relative_pose(
+                reference_combination[pair[0]],
+                reference_combination[pair[1]],
+            )
+            for pair in pair_names
+        }
+        reference_distances = {
+            pair: float(
+                np.linalg.norm(
+                    np.asarray(
+                        reference_combination[pair[1]].translation_xyz_m,
+                        dtype=float,
+                    )
+                    - np.asarray(
+                        reference_combination[pair[0]].translation_xyz_m,
+                        dtype=float,
+                    )
+                )
+            )
+            for pair in pair_names
+        }
+        first_emission = emission(
+            reference_combination,
+            reference_pairs,
+            reference_distances,
+        )
+        previous_costs = np.full(
+            len(combinations_by_frame[0]),
+            np.inf,
+            dtype=float,
+        )
+        previous_costs[reference_index] = first_emission[0]
+        back_pointers: list[list[int]] = []
+        for frame_index in range(1, len(combinations_by_frame)):
+            previous_combinations = combinations_by_frame[frame_index - 1]
+            combinations = combinations_by_frame[frame_index]
+            frame_emissions = [
+                emission(
+                    combination,
+                    reference_pairs,
+                    reference_distances,
+                )
+                for combination in combinations
+            ]
+            current_costs = np.full(len(combinations), np.inf, dtype=float)
+            frame_back_pointers: list[int] = []
+            for current_index, combination in enumerate(combinations):
+                transition_costs = [
+                    previous_costs[previous_index]
+                    + transition_cost(
+                        previous_combination,
+                        combination,
+                    )
+                    for previous_index, previous_combination in enumerate(
+                        previous_combinations
+                    )
+                ]
+                best_previous = int(np.argmin(transition_costs))
+                frame_back_pointers.append(best_previous)
+                current_costs[current_index] = (
+                    transition_costs[best_previous]
+                    + frame_emissions[current_index][0]
+                )
+            back_pointers.append(frame_back_pointers)
+            previous_costs = current_costs
+
+        final_index = int(np.argmin(previous_costs))
+        total = float(previous_costs[final_index])
+        path_indices = [final_index]
+        for frame_back_pointers in reversed(back_pointers):
+            path_indices.append(
+                frame_back_pointers[path_indices[-1]]
+            )
+        path_indices.reverse()
+        path = [
+            combinations[index]
+            for combinations, index in zip(
+                combinations_by_frame,
+                path_indices,
+            )
+        ]
+        if total < best_total:
+            best_total = total
+            best_path = path
+
+    if best_path is None:
+        raise RuntimeError("trajectory branch selection produced no path")
+
+    # The marker-to-marker mounting transforms are unknown, so the rigid
+    # reference must be estimated from the complete sweep.  Using frame zero
+    # as both the optimisation seed and the reported quality reference made
+    # one noisy endpoint frame look like drift in every other frame.  A
+    # rotation medoid and component-wise translation median are insensitive
+    # to that endpoint noise while still exposing a persistent mirror branch.
+    robust_reference_pairs: dict[
+        tuple[str, str], tuple[Rotation, np.ndarray]
+    ] = {}
+    for pair in pair_names:
+        pair_poses = [
+            _relative_pose(frame[pair[0]], frame[pair[1]])
+            for frame in best_path
+        ]
+        pair_rotations = [pose[0] for pose in pair_poses]
+        angular_costs = np.asarray(
+            [
+                sum(
+                    float((candidate.inv() * other).magnitude())
+                    for other in pair_rotations
+                )
+                for candidate in pair_rotations
+            ],
+            dtype=float,
+        )
+        rotation_medoid = pair_rotations[int(np.argmin(angular_costs))]
+        translation_median = np.median(
+            np.asarray([pose[1] for pose in pair_poses], dtype=float),
+            axis=0,
+        )
+        robust_reference_pairs[pair] = (
+            rotation_medoid,
+            translation_median,
+        )
+
+    rotation_drifts_by_frame: list[float] = []
+    translation_drifts_by_frame: list[float] = []
+    pair_distances_by_pair = {
+        pair: np.asarray(
+            [
+                np.linalg.norm(
+                    np.asarray(frame[pair[1]].translation_xyz_m, dtype=float)
+                    - np.asarray(
+                        frame[pair[0]].translation_xyz_m, dtype=float
+                    )
+                )
+                for frame in best_path
+            ],
+            dtype=float,
+        )
+        for pair in pair_names
+    }
+    robust_pair_distances = {
+        pair: float(np.median(distances))
+        for pair, distances in pair_distances_by_pair.items()
+    }
+    distance_drifts_by_frame: list[float] = []
+    for frame in best_path:
+        frame_rotation_drifts: list[float] = []
+        frame_translation_drifts: list[float] = []
+        frame_distance_drifts: list[float] = []
+        for pair, (
+            reference_rotation,
+            reference_translation,
+        ) in robust_reference_pairs.items():
+            rotation, translation = _relative_pose(
+                frame[pair[0]], frame[pair[1]]
+            )
+            frame_rotation_drifts.append(
+                float((reference_rotation.inv() * rotation).magnitude())
+            )
+            frame_translation_drifts.append(
+                float(np.linalg.norm(translation - reference_translation))
+            )
+            distance = float(
+                np.linalg.norm(
+                    np.asarray(frame[pair[1]].translation_xyz_m, dtype=float)
+                    - np.asarray(
+                        frame[pair[0]].translation_xyz_m, dtype=float
+                    )
+                )
+            )
+            frame_distance_drifts.append(
+                abs(distance - robust_pair_distances[pair])
+            )
+        rotation_drifts_by_frame.append(
+            max(frame_rotation_drifts, default=0.0)
+        )
+        translation_drifts_by_frame.append(
+            max(frame_translation_drifts, default=0.0)
+        )
+        distance_drifts_by_frame.append(
+            max(frame_distance_drifts, default=0.0)
+        )
+
+    rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float)
+    translation_drifts = np.asarray(
+        translation_drifts_by_frame, dtype=float
+    )
+    distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float)
+    return best_path, {
+        "total_cost": float(best_total),
+        "pair_geometry": geometry_mode,
+        "maximum_pair_rotation_drift_rad": float(
+            np.max(rotation_drifts, initial=0.0)
+        ),
+        "p95_pair_rotation_drift_rad": float(
+            np.percentile(rotation_drifts, 95.0)
+        ),
+        "median_pair_rotation_drift_rad": float(
+            np.median(rotation_drifts)
+        ),
+        "maximum_pair_translation_drift_m": float(
+            np.max(translation_drifts, initial=0.0)
+        ),
+        "p95_pair_translation_drift_m": float(
+            np.percentile(translation_drifts, 95.0)
+        ),
+        "maximum_pair_distance_drift_m": float(
+            np.max(distance_drifts, initial=0.0)
+        ),
+        "p95_pair_distance_drift_m": float(
+            np.percentile(distance_drifts, 95.0)
+        ),
+        "median_pair_distance_drift_m": float(
+            np.median(distance_drifts)
+        ),
+    }
+
+
+def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray:
+    matrix = np.asarray(camera_matrix, dtype=np.float64)
+    if matrix.shape != (3, 3):
+        raise ValueError("camera_matrix must have shape (3, 3)")
+    if not np.all(np.isfinite(matrix)):
+        raise ValueError("camera_matrix must be finite")
+    if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0:
+        raise ValueError("camera focal lengths must be positive")
+    return matrix
+
+
+def square_object_points(tag_size_m: float) -> np.ndarray:
+    """Return IPPE-square points matching apriltag_msgs corner order.
+
+    ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left.
+    OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in
+    the order below.
+    """
+    size = float(tag_size_m)
+    if not math.isfinite(size) or size <= 0.0:
+        raise ValueError("tag_size_m must be finite and positive")
+    half = size / 2.0
+    return np.asarray(
+        [
+            [-half, half, 0.0],
+            [half, half, 0.0],
+            [half, -half, 0.0],
+            [-half, -half, 0.0],
+        ],
+        dtype=np.float64,
+    )
+
+
+def solve_square_tag_ippe(
+    corners_xy: Sequence[Sequence[float]],
+    *,
+    tag_size_m: float,
+    camera_matrix: Sequence[Sequence[float]],
+) -> list[SquareTagPose]:
+    """Return every finite, positive-depth IPPE pose for one square tag."""
+    image_points = np.asarray(corners_xy, dtype=np.float64)
+    if image_points.shape != (4, 2):
+        raise ValueError("corners_xy must have shape (4, 2)")
+    if not np.all(np.isfinite(image_points)):
+        raise ValueError("corners_xy must be finite")
+    intrinsic = _as_camera_matrix(camera_matrix)
+    object_points = square_object_points(tag_size_m)
+    distortion = np.zeros((4, 1), dtype=np.float64)
+
+    solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric(
+        object_points,
+        image_points,
+        intrinsic,
+        distortion,
+        flags=cv2.SOLVEPNP_IPPE_SQUARE,
+    )
+    if not solved:
+        return []
+
+    candidates: list[SquareTagPose] = []
+    for rotation_vector, translation in zip(rotation_vectors, translations):
+        rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
+        translation_vector = np.asarray(translation, dtype=float).reshape(3)
+        camera_points = (
+            rotation_matrix @ object_points.T
+            + translation_vector.reshape(3, 1)
+        ).T
+        if np.min(camera_points[:, 2]) <= 0.0:
+            continue
+        projected, _ = cv2.projectPoints(
+            object_points,
+            rotation_vector,
+            translation_vector,
+            intrinsic,
+            distortion,
+        )
+        residual = projected.reshape(4, 2) - image_points
+        reprojection_error = float(
+            np.sqrt(np.mean(np.sum(residual * residual, axis=1)))
+        )
+        quaternion = Rotation.from_matrix(rotation_matrix).as_quat()
+        if not (
+            np.all(np.isfinite(quaternion))
+            and np.all(np.isfinite(translation_vector))
+            and math.isfinite(reprojection_error)
+        ):
+            continue
+        candidates.append(
+            SquareTagPose(
+                quaternion_xyzw=tuple(float(value) for value in quaternion),
+                translation_xyz_m=tuple(
+                    float(value) for value in translation_vector
+                ),
+                reprojection_error_px=reprojection_error,
+            )
+        )
+    return candidates
+
+
+def rotation_distance_rad(
+    first_xyzw: Sequence[float],
+    second_xyzw: Sequence[float],
+) -> float:
+    first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float))
+    second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float))
+    return float((first.inv() * second).magnitude())
+
+
+def select_continuous_pose(
+    candidates: Sequence[SquareTagPose],
+    *,
+    previous: SquareTagPose | None,
+    maximum_reprojection_error_px: float,
+    reprojection_tie_px: float,
+    maximum_pose_jump_rad: float,
+    maximum_translation_jump_m: float,
+    maximum_tag_tilt_rad: float,
+) -> tuple[SquareTagPose | None, str]:
+    """Select the best IPPE branch using image fit and temporal continuity."""
+    maximum_error = float(maximum_reprojection_error_px)
+    tie_error = float(reprojection_tie_px)
+    maximum_rotation = float(maximum_pose_jump_rad)
+    maximum_translation = float(maximum_translation_jump_m)
+    maximum_tilt = float(maximum_tag_tilt_rad)
+    if min(
+        maximum_error,
+        maximum_rotation,
+        maximum_translation,
+        maximum_tilt,
+    ) <= 0.0:
+        raise ValueError("PnP selection thresholds must be positive")
+    if tie_error < 0.0:
+        raise ValueError("reprojection_tie_px must be non-negative")
+
+    eligible: list[SquareTagPose] = []
+    for candidate in candidates:
+        if candidate.reprojection_error_px > maximum_error:
+            continue
+        normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2]
+        tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0)))
+        if tilt > maximum_tilt:
+            continue
+        eligible.append(candidate)
+    if not eligible:
+        return None, "no_pose_within_reprojection_or_tilt_limit"
+
+    eligible.sort(key=lambda item: item.reprojection_error_px)
+    best = eligible[0]
+    if previous is None:
+        return best, ""
+
+    # Temporal continuity must only break a genuine planar-PnP tie.  The old
+    # implementation normalised reprojection error by the permissive 1.5 px
+    # rejection limit, which allowed a stale mirror branch at 0.25 px to beat
+    # the true branch at e.g. 0.05 px merely because it was closer to the
+    # preceding (already wrong) pose.  Once one IPPE solution has a meaningful
+    # image-fit advantage, trust it and allow the tracker to leave the stale
+    # branch even if that correction is a large pose jump.
+    competitive = [
+        candidate
+        for candidate in eligible
+        if candidate.reprojection_error_px
+        <= best.reprojection_error_px + tie_error
+    ]
+    if len(competitive) == 1:
+        return best, ""
+
+    previous_translation = np.asarray(previous.translation_xyz_m, dtype=float)
+    scored: list[tuple[float, SquareTagPose]] = []
+    for candidate in competitive:
+        rotation_jump = rotation_distance_rad(
+            previous.quaternion_xyzw,
+            candidate.quaternion_xyzw,
+        )
+        translation_jump = float(
+            np.linalg.norm(
+                np.asarray(candidate.translation_xyz_m, dtype=float)
+                - previous_translation
+            )
+        )
+        if (
+            rotation_jump > maximum_rotation
+            or translation_jump > maximum_translation
+        ):
+            continue
+        score = (
+            (
+                candidate.reprojection_error_px
+                - best.reprojection_error_px
+            )
+            / max(tie_error, np.finfo(float).eps)
+            + rotation_jump / maximum_rotation
+            + translation_jump / maximum_translation
+        )
+        scored.append((float(score), candidate))
+    if not scored:
+        return None, "pose_jump"
+
+    selected = min(scored, key=lambda item: item[0])[1]
+    previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float)
+    selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float)
+    if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0:
+        selected = replace(
+            selected,
+            quaternion_xyzw=tuple(
+                float(value) for value in -selected_quaternion
+            ),
+        )
+    return selected, ""
+
+
+class SquareTagPoseTracker:
+    """Maintain the selected planar-PnP branch independently for each tag."""
+
+    def __init__(
+        self,
+        *,
+        maximum_reprojection_error_px: float,
+        reprojection_tie_px: float,
+        maximum_pose_jump_rad: float,
+        maximum_translation_jump_m: float,
+        maximum_tag_tilt_rad: float,
+        reset_after_seconds: float,
+    ) -> None:
+        self.maximum_reprojection_error_px = float(
+            maximum_reprojection_error_px
+        )
+        self.reprojection_tie_px = float(reprojection_tie_px)
+        self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
+        self.maximum_translation_jump_m = float(maximum_translation_jump_m)
+        self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad)
+        self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000)
+        if self.reset_after_ns <= 0:
+            raise ValueError("reset_after_seconds must be positive")
+        self._previous: dict[str, tuple[int, SquareTagPose]] = {}
+        self.last_candidates_by_role: dict[
+            str, tuple[SquareTagPose, ...]
+        ] = {}
+        self.branch_correction_counts: dict[str, int] = {}
+
+    def reset(self) -> None:
+        self._previous.clear()
+        self.last_candidates_by_role.clear()
+        self.branch_correction_counts.clear()
+
+    def estimate(
+        self,
+        role: str,
+        corners_xy: Sequence[Sequence[float]],
+        *,
+        tag_size_m: float,
+        camera_matrix: Sequence[Sequence[float]],
+        stamp_ns: int,
+        reprojection_tie_px: float | None = None,
+    ) -> tuple[SquareTagPose | None, str]:
+        try:
+            candidates = solve_square_tag_ippe(
+                corners_xy,
+                tag_size_m=tag_size_m,
+                camera_matrix=camera_matrix,
+            )
+        except (ValueError, cv2.error):
+            self.last_candidates_by_role[str(role)] = ()
+            return None, "pnp_solve_failed"
+        if not candidates:
+            self.last_candidates_by_role[str(role)] = ()
+            return None, "pnp_solve_failed"
+        usable_candidates: list[SquareTagPose] = []
+        for candidate in candidates:
+            normal = Rotation.from_quat(
+                candidate.quaternion_xyzw
+            ).as_matrix()[:, 2]
+            tilt = math.acos(
+                float(np.clip(abs(normal[2]), 0.0, 1.0))
+            )
+            if (
+                candidate.reprojection_error_px
+                <= self.maximum_reprojection_error_px
+                and tilt <= self.maximum_tag_tilt_rad
+            ):
+                usable_candidates.append(candidate)
+        self.last_candidates_by_role[str(role)] = tuple(
+            usable_candidates
+        )
+        if not usable_candidates:
+            return None, "no_pose_within_reprojection_or_tilt_limit"
+
+        previous_record = self._previous.get(str(role))
+        previous: SquareTagPose | None = None
+        if previous_record is not None:
+            previous_stamp, previous_pose = previous_record
+            elapsed = int(stamp_ns) - previous_stamp
+            if 0 <= elapsed <= self.reset_after_ns:
+                previous = previous_pose
+
+        selected, reason = select_continuous_pose(
+            usable_candidates,
+            previous=previous,
+            maximum_reprojection_error_px=(
+                self.maximum_reprojection_error_px
+            ),
+            reprojection_tie_px=(
+                self.reprojection_tie_px
+                if reprojection_tie_px is None
+                else float(reprojection_tie_px)
+            ),
+            maximum_pose_jump_rad=self.maximum_pose_jump_rad,
+            maximum_translation_jump_m=self.maximum_translation_jump_m,
+            maximum_tag_tilt_rad=self.maximum_tag_tilt_rad,
+        )
+        if selected is not None:
+            if previous is not None:
+                rotation_jump = rotation_distance_rad(
+                    previous.quaternion_xyzw,
+                    selected.quaternion_xyzw,
+                )
+                translation_jump = float(
+                    np.linalg.norm(
+                        np.asarray(selected.translation_xyz_m, dtype=float)
+                        - np.asarray(
+                            previous.translation_xyz_m,
+                            dtype=float,
+                        )
+                    )
+                )
+                if (
+                    rotation_jump > self.maximum_pose_jump_rad
+                    or translation_jump
+                    > self.maximum_translation_jump_m
+                ):
+                    key = str(role)
+                    self.branch_correction_counts[key] = (
+                        self.branch_correction_counts.get(key, 0) + 1
+                    )
+            self._previous[str(role)] = (int(stamp_ns), selected)
+        return selected, reason
+
+
+class SquareTagGroupPoseTracker:
+    """Choose all tag branches together using thumb-chain continuity.
+
+    A 30 px planar tag has two IPPE solutions whose reprojection errors can
+    exchange order from one frame to the next.  Tracking each tag
+    independently can therefore choose an incompatible pair for a relative
+    joint such as T4->T5.  This tracker enumerates the small Cartesian product
+    (at most 2**4 combinations) and favours the combination that keeps both
+    the camera poses and all adjacent relative poses continuous.
+    """
+
+    def __init__(
+        self,
+        *,
+        roles: Sequence[str],
+        adjacent_pairs: Sequence[tuple[str, str]],
+        maximum_pose_jump_rad: float,
+        maximum_translation_jump_m: float,
+        relative_rotation_scale_rad: float,
+        relative_translation_scale_m: float,
+        reprojection_scale_px: float,
+        reprojection_weight: float,
+        reset_after_seconds: float,
+    ) -> None:
+        self.roles = tuple(str(role) for role in roles)
+        self.adjacent_pairs = tuple(
+            (str(parent), str(child))
+            for parent, child in adjacent_pairs
+        )
+        if not self.roles or len(set(self.roles)) != len(self.roles):
+            raise ValueError("roles must be non-empty and unique")
+        if any(
+            parent not in self.roles or child not in self.roles
+            for parent, child in self.adjacent_pairs
+        ):
+            raise ValueError("adjacent_pairs must reference roles")
+        self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
+        self.maximum_translation_jump_m = float(
+            maximum_translation_jump_m
+        )
+        self.relative_rotation_scale_rad = float(
+            relative_rotation_scale_rad
+        )
+        self.relative_translation_scale_m = float(
+            relative_translation_scale_m
+        )
+        self.reprojection_scale_px = float(reprojection_scale_px)
+        self.reprojection_weight = float(reprojection_weight)
+        reset_seconds = float(reset_after_seconds)
+        if min(
+            self.maximum_pose_jump_rad,
+            self.maximum_translation_jump_m,
+            self.relative_rotation_scale_rad,
+            self.relative_translation_scale_m,
+            self.reprojection_scale_px,
+            reset_seconds,
+        ) <= 0.0:
+            raise ValueError("group tracking scales must be positive")
+        if self.reprojection_weight < 0.0:
+            raise ValueError("reprojection_weight must be non-negative")
+        self.reset_after_ns = int(reset_seconds * 1_000_000_000)
+        self._previous: dict[str, SquareTagPose] = {}
+        self._previous_stamp_ns: int | None = None
+        self.branch_correction_counts: dict[str, int] = {}
+
+    def reset(self) -> None:
+        self._previous.clear()
+        self._previous_stamp_ns = None
+        self.branch_correction_counts.clear()
+
+    def select(
+        self,
+        candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
+        *,
+        stamp_ns: int,
+    ) -> tuple[dict[str, SquareTagPose] | None, str]:
+        """Return one mutually consistent pose for every configured role."""
+        candidate_lists = [
+            tuple(candidates_by_role.get(role, ()))
+            for role in self.roles
+        ]
+        if any(not candidates for candidates in candidate_lists):
+            return None, "group_missing_pose_candidates"
+
+        combinations = [
+            dict(zip(self.roles, combination))
+            for combination in product(*candidate_lists)
+        ]
+        minimum_errors = {
+            role: min(
+                candidate.reprojection_error_px
+                for candidate in candidates
+            )
+            for role, candidates in zip(self.roles, candidate_lists)
+        }
+        stamp = int(stamp_ns)
+        previous_is_fresh = (
+            self._previous_stamp_ns is not None
+            and 0 <= stamp - self._previous_stamp_ns
+            <= self.reset_after_ns
+            and set(self._previous) == set(self.roles)
+        )
+
+        if not previous_is_fresh:
+            selected = min(
+                combinations,
+                key=lambda combination: sum(
+                    pose.reprojection_error_px
+                    for pose in combination.values()
+                ),
+            )
+        else:
+            previous_relative = {
+                pair: _relative_pose(
+                    self._previous[pair[0]],
+                    self._previous[pair[1]],
+                )
+                for pair in self.adjacent_pairs
+            }
+            scored: list[tuple[float, dict[str, SquareTagPose]]] = []
+            for combination in combinations:
+                absolute_rotation_motion = 0.0
+                absolute_translation_motion = 0.0
+                rejected = False
+                for role in self.roles:
+                    rotation_motion = rotation_distance_rad(
+                        self._previous[role].quaternion_xyzw,
+                        combination[role].quaternion_xyzw,
+                    )
+                    translation_motion = float(
+                        np.linalg.norm(
+                            np.asarray(
+                                combination[role].translation_xyz_m,
+                                dtype=float,
+                            )
+                            - np.asarray(
+                                self._previous[role].translation_xyz_m,
+                                dtype=float,
+                            )
+                        )
+                    )
+                    if (
+                        rotation_motion > self.maximum_pose_jump_rad
+                        or translation_motion
+                        > self.maximum_translation_jump_m
+                    ):
+                        rejected = True
+                        break
+                    absolute_rotation_motion += rotation_motion
+                    absolute_translation_motion += translation_motion
+                if rejected:
+                    continue
+
+                relative_rotation_motion = 0.0
+                relative_translation_motion = 0.0
+                for pair in self.adjacent_pairs:
+                    rotation, translation = _relative_pose(
+                        combination[pair[0]],
+                        combination[pair[1]],
+                    )
+                    old_rotation, old_translation = previous_relative[pair]
+                    relative_rotation_motion += float(
+                        (old_rotation.inv() * rotation).magnitude()
+                    )
+                    relative_translation_motion += float(
+                        np.linalg.norm(translation - old_translation)
+                    )
+
+                reprojection_penalty = sum(
+                    max(
+                        0.0,
+                        combination[role].reprojection_error_px
+                        - minimum_errors[role],
+                    )
+                    for role in self.roles
+                ) / self.reprojection_scale_px
+                score = (
+                    absolute_rotation_motion
+                    / self.maximum_pose_jump_rad
+                    + absolute_translation_motion
+                    / self.maximum_translation_jump_m
+                    + relative_rotation_motion
+                    / self.relative_rotation_scale_rad
+                    + relative_translation_motion
+                    / self.relative_translation_scale_m
+                    + self.reprojection_weight * reprojection_penalty
+                )
+                scored.append((float(score), combination))
+
+            if not scored:
+                return None, "group_pose_jump"
+            selected = min(scored, key=lambda item: item[0])[1]
+
+        aligned: dict[str, SquareTagPose] = {}
+        for role in self.roles:
+            pose = selected[role]
+            if previous_is_fresh:
+                old_quaternion = np.asarray(
+                    self._previous[role].quaternion_xyzw,
+                    dtype=float,
+                )
+                quaternion = np.asarray(
+                    pose.quaternion_xyzw,
+                    dtype=float,
+                )
+                if float(np.dot(old_quaternion, quaternion)) < 0.0:
+                    pose = replace(
+                        pose,
+                        quaternion_xyzw=tuple(
+                            float(value) for value in -quaternion
+                        ),
+                    )
+            best_reprojection = min(
+                candidate_lists[self.roles.index(role)],
+                key=lambda candidate: candidate.reprojection_error_px,
+            )
+            if pose != best_reprojection:
+                self.branch_correction_counts[role] = (
+                    self.branch_correction_counts.get(role, 0) + 1
+                )
+            aligned[role] = pose
+
+        self._previous = aligned
+        self._previous_stamp_ns = stamp
+        return dict(aligned), ""

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py

@@ -0,0 +1,87 @@
+"""Crash-safe session storage for hardware calibration."""
+
+from __future__ import annotations
+
+import json
+import os
+from pathlib import Path
+from typing import Any, Iterable, Mapping
+
+
+def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None:
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    with temporary.open("w", encoding="utf-8") as stream:
+        json.dump(payload, stream, ensure_ascii=False, indent=2)
+        stream.write("\n")
+        stream.flush()
+        os.fsync(stream.fileno())
+    os.replace(temporary, destination)
+
+
+def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    line = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
+    with destination.open("a", encoding="utf-8") as stream:
+        stream.write(line + "\n")
+        stream.flush()
+        os.fsync(stream.fileno())
+
+
+def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
+    source = Path(path)
+    if not source.exists():
+        return []
+    records: list[dict[str, Any]] = []
+    with source.open("r", encoding="utf-8") as stream:
+        lines = stream.readlines()
+        nonempty_lines = [
+            index for index, line in enumerate(lines, 1) if line.strip()
+        ]
+        last_nonempty_line = nonempty_lines[-1] if nonempty_lines else 0
+        for line_number, line in enumerate(lines, 1):
+            if not line.strip():
+                continue
+            try:
+                value = json.loads(line)
+            except json.JSONDecodeError as error:
+                if line_number == last_nonempty_line:
+                    break
+                raise ValueError(
+                    f"{source}:{line_number}: invalid JSONL record"
+                ) from error
+            if not isinstance(value, dict):
+                raise ValueError(f"{source}:{line_number}: record must be an object")
+            records.append(value)
+    return records
+
+
+def load_json(path: str | Path) -> dict[str, Any] | None:
+    source = Path(path)
+    if not source.exists():
+        return None
+    with source.open("r", encoding="utf-8") as stream:
+        value = json.load(stream)
+    if not isinstance(value, dict):
+        raise ValueError(f"{source} must contain a JSON object")
+    return value
+
+
+def completed_scan_keys(
+    records: Iterable[Mapping[str, Any]],
+) -> set[tuple[str, int, str, int]]:
+    keys: set[tuple[str, int, str, int]] = set()
+    for record in records:
+        if record.get("kind", "sample") != "sample":
+            continue
+        keys.add(
+            (
+                str(record["phase"]),
+                int(record["cycle"]),
+                str(record["direction"]),
+                int(record["command_u8"]),
+            )
+        )
+    return keys

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py

@@ -0,0 +1,754 @@
+"""Pure helpers for image-trajectory CMC zero and travel measurement."""
+
+from __future__ import annotations
+
+import math
+from typing import Any, Mapping, Sequence
+
+import cv2
+import numpy as np
+from scipy.optimize import least_squares
+
+
+def wrap_angle_rad(angle_rad: float) -> float:
+    """Wrap one finite angle to [-pi, pi)."""
+    angle = float(angle_rad)
+    if not math.isfinite(angle):
+        raise ValueError("angle must be finite")
+    return (angle + math.pi) % (2.0 * math.pi) - math.pi
+
+
+def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float:
+    """Return the wrapped signed rotation from reference to angle."""
+    return wrap_angle_rad(float(angle_rad) - float(reference_rad))
+
+
+def tag_x_axis_angle_rad(
+    corners_xy: Sequence[Sequence[float]],
+) -> float:
+    """Return the ordered AprilTag +x direction in a y-up image frame."""
+    corners = np.asarray(corners_xy, dtype=float)
+    if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+        raise ValueError("corners_xy must contain four finite xy points")
+    x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
+    if float(np.linalg.norm(x_axis)) < 1.0e-9:
+        raise ValueError("tag x-axis is degenerate")
+    return wrap_angle_rad(
+        math.atan2(-float(x_axis[1]), float(x_axis[0]))
+    )
+
+
+def circular_mean_rad(angles_rad: Sequence[float]) -> float:
+    """Return the circular mean of at least one finite angle."""
+    angles = np.asarray(angles_rad, dtype=float)
+    if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
+        raise ValueError("angles_rad must contain finite angles")
+    vector = complex(
+        float(np.mean(np.cos(angles))),
+        float(np.mean(np.sin(angles))),
+    )
+    if abs(vector) < 1.0e-12:
+        raise ValueError("angles do not have a unique circular mean")
+    return wrap_angle_rad(math.atan2(vector.imag, vector.real))
+
+
+def circular_median_rad(angles_rad: Sequence[float]) -> float:
+    """Return a robust circular median around the circular mean."""
+    angles = np.asarray(angles_rad, dtype=float)
+    reference = circular_mean_rad(angles)
+    deltas = np.asarray(
+        [signed_angle_difference_rad(value, reference) for value in angles],
+        dtype=float,
+    )
+    return wrap_angle_rad(reference + float(np.median(deltas)))
+
+
+def circular_std_rad(angles_rad: Sequence[float]) -> float:
+    """Return circular standard deviation in radians."""
+    angles = np.asarray(angles_rad, dtype=float)
+    if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
+        raise ValueError("angles_rad must contain finite angles")
+    resultant = float(
+        math.hypot(
+            float(np.mean(np.cos(angles))),
+            float(np.mean(np.sin(angles))),
+        )
+    )
+    resultant = min(1.0, max(1.0e-15, resultant))
+    return math.sqrt(max(0.0, -2.0 * math.log(resultant)))
+
+
+def maximum_pairwise_angle_difference_rad(
+    angles_rad: Sequence[float],
+) -> float:
+    """Return the largest wrapped distance between any two angles."""
+    angles = [float(value) for value in angles_rad]
+    if not angles or not all(math.isfinite(value) for value in angles):
+        raise ValueError("angles_rad must contain finite angles")
+    return max(
+        (
+            abs(signed_angle_difference_rad(left, right))
+            for index, left in enumerate(angles)
+            for right in angles[index + 1:]
+        ),
+        default=0.0,
+    )
+
+
+def detect_reference_alignment_line(
+    image: np.ndarray,
+    *,
+    reference_y_px: float,
+    roi_y_min_ratio: float,
+    roi_y_max_ratio: float,
+    minimum_length_ratio: float,
+    maximum_candidate_angle_rad: float,
+) -> dict[str, Any] | None:
+    """Detect the best near-horizontal physical reference edge.
+
+    The returned angle uses image +x with y pointing up, matching the public
+    table-projected angle convention.  AprilTag corners and orientation are
+    deliberately not used.  Among sufficiently long Hough segments, the
+    edge nearest the fixed red reference line is preferred.
+    """
+    pixels = np.asarray(image)
+    if pixels.ndim not in {2, 3} or pixels.shape[0] < 8 or pixels.shape[1] < 8:
+        raise ValueError("image must be a non-empty grayscale or BGR array")
+    if pixels.ndim == 3 and pixels.shape[2] not in {3, 4}:
+        raise ValueError("image must have 3 BGR or 4 BGRA channels")
+    height, width = pixels.shape[:2]
+    if not math.isfinite(reference_y_px) or not 0.0 <= reference_y_px < height:
+        raise ValueError("reference_y_px must be inside the image")
+    if not 0.0 <= roi_y_min_ratio < roi_y_max_ratio <= 1.0:
+        raise ValueError("alignment ROI ratios must satisfy 0 <= min < max <= 1")
+    if not 0.0 < minimum_length_ratio <= 1.0:
+        raise ValueError("minimum_length_ratio must be in (0, 1]")
+    if not 0.0 < maximum_candidate_angle_rad < math.pi / 2.0:
+        raise ValueError("maximum_candidate_angle_rad must be in (0, pi/2)")
+
+    if pixels.ndim == 2:
+        gray = pixels.astype(np.uint8, copy=False)
+    elif pixels.shape[2] == 4:
+        gray = cv2.cvtColor(pixels, cv2.COLOR_BGRA2GRAY)
+    else:
+        gray = cv2.cvtColor(pixels, cv2.COLOR_BGR2GRAY)
+    gray = cv2.GaussianBlur(gray, (5, 5), 0.0)
+    edges = cv2.Canny(gray, 50, 150)
+
+    roi_top = max(0, min(height - 1, int(round(height * roi_y_min_ratio))))
+    roi_bottom = max(
+        roi_top + 1,
+        min(height, int(round(height * roi_y_max_ratio))),
+    )
+    roi_edges = edges[roi_top:roi_bottom, :]
+    minimum_length_px = max(8, int(round(width * minimum_length_ratio)))
+    hough_threshold = max(25, int(round(minimum_length_px * 0.20)))
+    lines = cv2.HoughLinesP(
+        roi_edges,
+        1.0,
+        math.pi / 720.0,
+        threshold=hough_threshold,
+        minLineLength=minimum_length_px,
+        maxLineGap=max(10, int(round(width * 0.025))),
+    )
+    if lines is None:
+        return None
+
+    candidates: list[tuple[float, dict[str, Any]]] = []
+    image_centre_x = 0.5 * float(width - 1)
+    for raw_line in lines[:, 0, :]:
+        x1, y1, x2, y2 = (float(value) for value in raw_line)
+        y1 += roi_top
+        y2 += roi_top
+        if x2 < x1:
+            x1, x2 = x2, x1
+            y1, y2 = y2, y1
+        delta_x = x2 - x1
+        delta_y = y2 - y1
+        if delta_x < 1.0:
+            continue
+        length = math.hypot(delta_x, delta_y)
+        if length < minimum_length_px:
+            continue
+        angle_rad = math.atan2(-delta_y, delta_x)
+        if abs(angle_rad) > maximum_candidate_angle_rad:
+            continue
+        slope = delta_y / delta_x
+        centre_y = y1 + slope * (image_centre_x - x1)
+        offset_px = centre_y - float(reference_y_px)
+        full_y_left = y1 - slope * x1
+        full_y_right = y1 + slope * (float(width - 1) - x1)
+        normalized_angle = abs(angle_rad) / maximum_candidate_angle_rad
+        normalized_offset = abs(offset_px) / float(height)
+        normalized_length = min(1.5, length / float(width))
+        score = (
+            normalized_offset
+            + 0.02 * normalized_angle
+            - 0.10 * normalized_length
+        )
+        candidates.append(
+            (
+                score,
+                {
+                    "line_xyxy_px": [
+                        0.0,
+                        float(full_y_left),
+                        float(width - 1),
+                        float(full_y_right),
+                    ],
+                    "angle_rad": float(angle_rad),
+                    "vertical_offset_px": float(offset_px),
+                    "segment_length_px": float(length),
+                    "reference_y_px": float(reference_y_px),
+                    "roi_y_px": [int(roi_top), int(roi_bottom)],
+                },
+            )
+        )
+    if not candidates:
+        return None
+    return min(candidates, key=lambda item: item[0])[1]
+
+
+def summarize_zero_frames(
+    frames: Sequence[Mapping[str, float]],
+) -> dict[str, float]:
+    """Summarize one stable command-255 capture window.
+
+    Image coordinates use x-right/y-down.  The T3-minus-T0 centre is retained
+    so a later trajectory-circle fit is insensitive to camera/hand translation.
+    """
+    if not frames:
+        raise ValueError("at least one frame is required")
+    required = (
+        "t0_x_px",
+        "t0_y_px",
+        "t3_x_px",
+        "t3_y_px",
+    )
+    values: dict[str, list[float]] = {name: [] for name in required}
+    for frame in frames:
+        for name in required:
+            value = float(frame[name])
+            if not math.isfinite(value):
+                raise ValueError(f"{name} must be finite")
+            values[name].append(value)
+
+    t0_xy = np.column_stack((values["t0_x_px"], values["t0_y_px"]))
+    t3_xy = np.column_stack((values["t3_x_px"], values["t3_y_px"]))
+    relative_xy = t3_xy - t0_xy
+    relative_median = np.median(relative_xy, axis=0)
+    relative_error = np.linalg.norm(relative_xy - relative_median, axis=1)
+    t0_median = np.median(t0_xy, axis=0)
+    t3_median = np.median(t3_xy, axis=0)
+    return {
+        "relative_x_px": float(relative_median[0]),
+        "relative_y_px": float(relative_median[1]),
+        "t0_x_px": float(t0_median[0]),
+        "t0_y_px": float(t0_median[1]),
+        "t3_x_px": float(t3_median[0]),
+        "t3_y_px": float(t3_median[1]),
+        "relative_position_rms_px": float(
+            math.sqrt(float(np.mean(np.square(relative_error))))
+        ),
+    }
+
+
+def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]:
+    """Fit a geometric circle with robust nonlinear least squares."""
+    points = np.asarray(points_xy, dtype=float)
+    if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3:
+        raise ValueError("circle fit requires at least three 2D points")
+    origin = np.mean(points, axis=0)
+    centered = points - origin
+    matrix = np.column_stack(
+        (2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points)))
+    )
+    rhs = np.sum(np.square(centered), axis=1)
+    solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None)
+    if rank < 3:
+        raise ValueError("trajectory points are degenerate")
+    centre0 = solution[:2] + origin
+    radius0_squared = (
+        float(solution[2]) + float(np.dot(solution[:2], solution[:2]))
+    )
+    if radius0_squared <= 0.0:
+        raise ValueError("trajectory circle radius is invalid")
+    radius0 = math.sqrt(radius0_squared)
+
+    def residual(parameters: np.ndarray) -> np.ndarray:
+        centre = parameters[:2]
+        radius = parameters[2]
+        return np.linalg.norm(points - centre, axis=1) - radius
+
+    initial_error = residual(np.asarray([*centre0, radius0], dtype=float))
+    robust_scale = max(
+        0.25,
+        1.4826
+        * float(
+            np.median(
+                np.abs(initial_error - float(np.median(initial_error)))
+            )
+        ),
+    )
+    result = least_squares(
+        residual,
+        np.asarray([*centre0, radius0], dtype=float),
+        loss="soft_l1",
+        f_scale=robust_scale,
+        max_nfev=1000,
+    )
+    centre = np.asarray(result.x[:2], dtype=float)
+    radius = float(abs(result.x[2]))
+    if not result.success or not np.all(np.isfinite(centre)):
+        raise ValueError("trajectory circle optimization failed")
+    if not math.isfinite(radius) or radius <= 0.0:
+        raise ValueError("trajectory circle radius is invalid")
+    return centre, radius
+
+
+def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float:
+    angles = np.mod(
+        np.arctan2(
+            points_xy[:, 1] - centre_xy[1],
+            points_xy[:, 0] - centre_xy[0],
+        ),
+        2.0 * math.pi,
+    )
+    if len(angles) < 2:
+        return 0.0
+    angles = np.sort(angles)
+    gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi])
+    return float(2.0 * math.pi - np.max(gaps))
+
+
+def fit_image_circle_trajectory(
+    observations: Sequence[Mapping[str, float]],
+    *,
+    bin_size_u8: float,
+    minimum_frames: int,
+    minimum_bins: int,
+    minimum_state_span_u8: float,
+    minimum_radius_px: float,
+    minimum_arc_rad: float,
+    maximum_radial_rms_px: float,
+    maximum_p95_radial_error_px: float,
+) -> dict[str, Any]:
+    """Fit the T3-minus-T0 image trajectory to a robust circle.
+
+    Repeated detector frames at the same hand state are collapsed into state
+    bins before fitting so slow endpoints cannot dominate the circle.
+    """
+    if len(observations) < minimum_frames:
+        raise ValueError(
+            f"trajectory_frames_too_few:{len(observations)}/{minimum_frames}"
+        )
+    required = ("state_u8", "t0_x_px", "t0_y_px", "t3_x_px", "t3_y_px")
+    rows: list[tuple[float, float, float]] = []
+    for observation in observations:
+        values = [float(observation[name]) for name in required]
+        if not all(math.isfinite(value) for value in values):
+            continue
+        state, t0_x, t0_y, t3_x, t3_y = values
+        rows.append((state, t3_x - t0_x, t3_y - t0_y))
+    if len(rows) < minimum_frames:
+        raise ValueError(
+            f"trajectory_valid_frames_too_few:{len(rows)}/{minimum_frames}"
+        )
+
+    samples = np.asarray(rows, dtype=float)
+    state_span = float(np.ptp(samples[:, 0]))
+    if state_span < minimum_state_span_u8:
+        raise ValueError(
+            "trajectory_state_span_too_small:"
+            f"{state_span:.1f}/{minimum_state_span_u8:.1f}"
+        )
+
+    bins: dict[int, list[np.ndarray]] = {}
+    for state, x_value, y_value in samples:
+        key = int(round(float(state) / bin_size_u8))
+        bins.setdefault(key, []).append(np.asarray([x_value, y_value]))
+    binned_points = np.asarray(
+        [np.median(np.asarray(values), axis=0) for _, values in sorted(bins.items())],
+        dtype=float,
+    )
+    if len(binned_points) < minimum_bins:
+        raise ValueError(
+            f"trajectory_bins_too_few:{len(binned_points)}/{minimum_bins}"
+        )
+
+    centre, radius = _fit_circle(binned_points)
+    radial_error = np.abs(
+        np.linalg.norm(binned_points - centre, axis=1) - radius
+    )
+    median_error = float(np.median(radial_error))
+    mad = 1.4826 * float(np.median(np.abs(radial_error - median_error)))
+    inlier_limit = max(0.75, median_error + 3.5 * max(mad, 0.1))
+    inliers = radial_error <= inlier_limit
+    if int(np.count_nonzero(inliers)) >= max(6, minimum_bins // 2):
+        centre, radius = _fit_circle(binned_points[inliers])
+        radial_error = np.abs(
+            np.linalg.norm(binned_points - centre, axis=1) - radius
+        )
+    radial_rms = math.sqrt(float(np.mean(np.square(radial_error))))
+    radial_p95 = float(np.percentile(radial_error, 95.0))
+    arc_rad = _trajectory_arc_rad(binned_points, centre)
+
+    failures: list[str] = []
+    if radius < minimum_radius_px:
+        failures.append(f"radius={radius:.2f}px")
+    if arc_rad < minimum_arc_rad:
+        failures.append(f"arc={math.degrees(arc_rad):.2f}deg")
+    if radial_rms > maximum_radial_rms_px:
+        failures.append(f"rms={radial_rms:.2f}px")
+    if radial_p95 > maximum_p95_radial_error_px:
+        failures.append(f"p95={radial_p95:.2f}px")
+    if failures:
+        raise ValueError("trajectory_circle_quality_failed:" + ",".join(failures))
+
+    return {
+        "method": "t3_minus_t0_image_trajectory_circle",
+        "centre_relative_xy_px": [float(centre[0]), float(centre[1])],
+        "radius_px": radius,
+        "radial_rms_px": radial_rms,
+        "radial_p95_px": radial_p95,
+        "arc_rad": arc_rad,
+        "state_span_u8": state_span,
+        "frames": len(rows),
+        "bins": len(binned_points),
+        "passed": True,
+    }
+
+
+def measure_zero_from_circle(
+    circle: Mapping[str, Any],
+    zero_summary: Mapping[str, float],
+) -> dict[str, float]:
+    """Measure the fixed inward radial angle from a fitted trajectory circle.
+
+    The public zero convention is always T3 centre -> trajectory centre.
+    Neither AprilTag corner ordering nor the printed T3 +x direction is used,
+    so rotating or mounting T3 upside down cannot flip the zero by pi.
+    """
+    centre = np.asarray(circle["centre_relative_xy_px"], dtype=float)
+    point = np.asarray(
+        [
+            float(zero_summary["relative_x_px"]),
+            float(zero_summary["relative_y_px"]),
+        ],
+        dtype=float,
+    )
+    inward = centre - point
+    distance = float(np.linalg.norm(inward))
+    if not math.isfinite(distance) or distance < 1.0e-9:
+        raise ValueError("zero point is at the fitted circle centre")
+    table_rad = wrap_angle_rad(
+        math.atan2(-float(inward[1]), float(inward[0]))
+    )
+    radius = float(circle["radius_px"])
+    return {
+        "table_rad": table_rad,
+        "zero_radial_error_px": abs(distance - radius),
+    }
+
+
+def build_trajectory_zero_angle_payload(
+    *,
+    serial_number: str,
+    rounds: Sequence[Mapping[str, Mapping[str, float]]],
+    trajectory_quality: Mapping[str, Any],
+    maximum_round_difference_rad: float,
+    maximum_return_error_rad: float,
+    maximum_zero_radial_error_px: float,
+    detection_rate: float,
+    minimum_detection_rate: float,
+) -> tuple[dict[str, Any], dict[str, Any]]:
+    """Create the compact JSON and detailed circle-fit status report."""
+    if len(rounds) != 3:
+        raise ValueError("exactly three rounds are required")
+    if not 0.0 <= detection_rate <= 1.0:
+        raise ValueError("detection_rate must be in [0, 1]")
+
+    table_rounds: list[float] = []
+    return_errors: list[float] = []
+    zero_radial_errors: list[float] = []
+    for round_value in rounds:
+        before = round_value["zero_before"]
+        after = round_value["zero_after"]
+        table_rounds.append(
+            circular_mean_rad(
+                [float(before["table_rad"]), float(after["table_rad"])]
+            )
+        )
+        return_errors.append(
+            abs(
+                signed_angle_difference_rad(
+                    float(after["table_rad"]),
+                    float(before["table_rad"]),
+                )
+            )
+        )
+        zero_radial_errors.extend(
+            [
+                float(before["zero_radial_error_px"]),
+                float(after["zero_radial_error_px"]),
+            ]
+        )
+
+    table_difference = maximum_pairwise_angle_difference_rad(table_rounds)
+    passed = bool(
+        bool(trajectory_quality.get("passed", False))
+        and detection_rate >= minimum_detection_rate
+        and table_difference <= maximum_round_difference_rad
+        and max(return_errors) <= maximum_return_error_rad
+        and max(zero_radial_errors) <= maximum_zero_radial_error_px
+    )
+
+    payload: dict[str, Any] = {
+        "schema_version": 2,
+        "model": "G20",
+        "side": "left",
+        "serial_number": str(serial_number),
+        "joint": "thumb_cmc_pitch",
+        "angle_unit": "rad",
+        "zero_command_u8": 255,
+        "zero_angles": {
+            "table_projected_zero_rad": circular_median_rad(table_rounds),
+        },
+        "quality": {
+            "passed": passed,
+            "three_run_table_max_difference_rad": table_difference,
+        },
+    }
+    report: dict[str, Any] = {
+        "measurement_method": (
+            "t3_center_to_circle_centre_image_trajectory"
+        ),
+        "detection_rate": float(detection_rate),
+        "trajectory_circle": dict(trajectory_quality),
+        "table_rounds_rad": table_rounds,
+        "return_errors_rad": return_errors,
+        "zero_radial_errors_px": zero_radial_errors,
+        "passed": passed,
+    }
+    return payload, report
+
+
+def validate_zero_angle_payload(payload: Mapping[str, Any]) -> None:
+    """Validate the deliberately small public JSON schema."""
+    if set(payload) != {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "joint",
+        "angle_unit",
+        "zero_command_u8",
+        "zero_angles",
+        "quality",
+    }:
+        raise ValueError("unexpected top-level zero calibration fields")
+    if payload["schema_version"] != 2:
+        raise ValueError("schema_version must be 2")
+    if payload["model"] != "G20" or payload["side"] != "left":
+        raise ValueError("payload must describe the left G20")
+    if payload["joint"] != "thumb_cmc_pitch":
+        raise ValueError("payload must describe thumb_cmc_pitch")
+    if payload["angle_unit"] != "rad" or payload["zero_command_u8"] != 255:
+        raise ValueError("zero calibration must use radians and command 255")
+    zero_angles = payload["zero_angles"]
+    if set(zero_angles) != {"table_projected_zero_rad"}:
+        raise ValueError("unexpected zero angle fields")
+    if not all(math.isfinite(float(value)) for value in zero_angles.values()):
+        raise ValueError("zero angles must be finite")
+    quality = payload["quality"]
+    if set(quality) != {
+        "passed",
+        "three_run_table_max_difference_rad",
+    }:
+        raise ValueError("unexpected quality fields")
+    if not isinstance(quality["passed"], bool):
+        raise ValueError("quality.passed must be boolean")
+    for name in ("three_run_table_max_difference_rad",):
+        value = float(quality[name])
+        if not math.isfinite(value) or value < 0.0:
+            raise ValueError(f"quality.{name} must be finite and non-negative")
+
+
+def build_trajectory_zero_travel_payload(
+    *,
+    serial_number: str,
+    joint_name: str,
+    zero_command_u8: int,
+    travel_endpoint_command_u8: int,
+    rounds: Sequence[Mapping[str, Mapping[str, float]]],
+    trajectory_quality: Mapping[str, Any],
+    maximum_round_difference_rad: float,
+    maximum_travel_difference_rad: float,
+    minimum_travel_rad: float,
+    maximum_return_error_rad: float,
+    maximum_zero_radial_error_px: float,
+    detection_rate: float,
+    minimum_detection_rate: float,
+) -> tuple[dict[str, Any], dict[str, Any]]:
+    """Create a compact zero-and-travel payload from three circle sweeps."""
+    if joint_name != "thumb_cmc_roll":
+        raise ValueError("zero-and-travel payload only supports thumb_cmc_roll")
+    if len(rounds) != 3:
+        raise ValueError("exactly three rounds are required")
+    if not 0.0 <= detection_rate <= 1.0:
+        raise ValueError("detection_rate must be in [0, 1]")
+    if not 0 <= zero_command_u8 <= 255:
+        raise ValueError("zero_command_u8 must be in [0, 255]")
+    if not 0 <= travel_endpoint_command_u8 <= 255:
+        raise ValueError("travel_endpoint_command_u8 must be in [0, 255]")
+    if zero_command_u8 == travel_endpoint_command_u8:
+        raise ValueError("zero and travel endpoint commands must differ")
+
+    table_rounds: list[float] = []
+    endpoint_table_rounds: list[float] = []
+    signed_travel_rounds: list[float] = []
+    return_errors: list[float] = []
+    radial_errors: list[float] = []
+    for round_value in rounds:
+        before = round_value["zero_before"]
+        endpoint = round_value["travel_endpoint"]
+        after = round_value["zero_after"]
+        zero_table = circular_mean_rad(
+            [float(before["table_rad"]), float(after["table_rad"])]
+        )
+        endpoint_table = float(endpoint["table_rad"])
+        table_rounds.append(zero_table)
+        endpoint_table_rounds.append(endpoint_table)
+        signed_travel_rounds.append(
+            signed_angle_difference_rad(endpoint_table, zero_table)
+        )
+        return_errors.append(
+            abs(
+                signed_angle_difference_rad(
+                    float(after["table_rad"]),
+                    float(before["table_rad"]),
+                )
+            )
+        )
+        radial_errors.extend(
+            float(value["zero_radial_error_px"])
+            for value in (before, endpoint, after)
+        )
+
+    table_difference = maximum_pairwise_angle_difference_rad(table_rounds)
+    travel_difference = maximum_pairwise_angle_difference_rad(
+        signed_travel_rounds
+    )
+    signed_travel = circular_median_rad(signed_travel_rounds)
+    travel_range = float(np.median(np.abs(signed_travel_rounds)))
+    passed = bool(
+        bool(trajectory_quality.get("passed", False))
+        and detection_rate >= minimum_detection_rate
+        and table_difference <= maximum_round_difference_rad
+        and travel_difference <= maximum_travel_difference_rad
+        and travel_range >= minimum_travel_rad
+        and max(return_errors) <= maximum_return_error_rad
+        and max(radial_errors) <= maximum_zero_radial_error_px
+    )
+
+    payload: dict[str, Any] = {
+        "schema_version": 2,
+        "model": "G20",
+        "side": "left",
+        "serial_number": str(serial_number),
+        "joint": joint_name,
+        "angle_unit": "rad",
+        "zero_command_u8": int(zero_command_u8),
+        "travel_endpoint_command_u8": int(travel_endpoint_command_u8),
+        "zero_angles": {
+            "table_projected_zero_rad": circular_median_rad(table_rounds),
+        },
+        "travel": {
+            "signed_rad": signed_travel,
+            "range_rad": travel_range,
+        },
+        "quality": {
+            "passed": passed,
+            "three_run_table_max_difference_rad": table_difference,
+            "three_run_travel_max_difference_rad": travel_difference,
+        },
+    }
+    report: dict[str, Any] = {
+        "measurement_method": (
+            "t3_center_to_circle_centre_image_trajectory"
+        ),
+        "detection_rate": float(detection_rate),
+        "trajectory_circle": dict(trajectory_quality),
+        "table_zero_rounds_rad": table_rounds,
+        "endpoint_table_rounds_rad": endpoint_table_rounds,
+        "signed_travel_rounds_rad": signed_travel_rounds,
+        "return_errors_rad": return_errors,
+        "radial_errors_px": radial_errors,
+        "minimum_travel_rad": float(minimum_travel_rad),
+        "passed": passed,
+    }
+    return payload, report
+
+
+def validate_zero_travel_payload(payload: Mapping[str, Any]) -> None:
+    """Validate the public left-G20 CMC-roll zero-and-travel schema."""
+    if set(payload) != {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "joint",
+        "angle_unit",
+        "zero_command_u8",
+        "travel_endpoint_command_u8",
+        "zero_angles",
+        "travel",
+        "quality",
+    }:
+        raise ValueError("unexpected top-level zero-and-travel fields")
+    if payload["schema_version"] != 2:
+        raise ValueError("schema_version must be 2")
+    if payload["model"] != "G20" or payload["side"] != "left":
+        raise ValueError("payload must describe the left G20")
+    if payload["joint"] != "thumb_cmc_roll":
+        raise ValueError("payload must describe thumb_cmc_roll")
+    if payload["angle_unit"] != "rad":
+        raise ValueError("zero-and-travel calibration must use radians")
+    for name in ("zero_command_u8", "travel_endpoint_command_u8"):
+        value = payload[name]
+        if not isinstance(value, int) or not 0 <= value <= 255:
+            raise ValueError(f"{name} must be an integer in [0, 255]")
+    if payload["zero_command_u8"] == payload["travel_endpoint_command_u8"]:
+        raise ValueError("zero and travel endpoint commands must differ")
+
+    zero_angles = payload["zero_angles"]
+    if set(zero_angles) != {"table_projected_zero_rad"}:
+        raise ValueError("unexpected zero angle fields")
+    if not all(math.isfinite(float(value)) for value in zero_angles.values()):
+        raise ValueError("zero angles must be finite")
+
+    travel = payload["travel"]
+    if set(travel) != {"signed_rad", "range_rad"}:
+        raise ValueError("unexpected travel fields")
+    if not math.isfinite(float(travel["signed_rad"])):
+        raise ValueError("travel.signed_rad must be finite")
+    travel_range = float(travel["range_rad"])
+    if not math.isfinite(travel_range) or travel_range < 0.0:
+        raise ValueError("travel.range_rad must be finite and non-negative")
+
+    quality = payload["quality"]
+    if set(quality) != {
+        "passed",
+        "three_run_table_max_difference_rad",
+        "three_run_travel_max_difference_rad",
+    }:
+        raise ValueError("unexpected quality fields")
+    if not isinstance(quality["passed"], bool):
+        raise ValueError("quality.passed must be boolean")
+    for name in (
+        "three_run_table_max_difference_rad",
+        "three_run_travel_max_difference_rad",
+    ):
+        value = float(quality[name])
+        if not math.isfinite(value) or value < 0.0:
+            raise ValueError(f"quality.{name} must be finite and non-negative")

src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py

@@ -0,0 +1,238 @@
+"""Launch front-camera trajectory-circle CMC pitch zero measurement."""
+
+from __future__ import annotations
+
+from datetime import datetime
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def _launch_stack(context):
+    serial_number = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not serial_number
+        or serial_number == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
+        or serial_number in {".", ".."}
+    ):
+        raise RuntimeError(
+            "serial_number is required and may contain only letters, "
+            "digits, dot, underscore and dash"
+        )
+
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(
+        LaunchConfiguration("output_root").perform(context)
+    ).expanduser().resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        session_dir = output_root / serial_number / timestamp
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    tag_config = LaunchConfiguration("tag_config").perform(context)
+    zero_config = LaunchConfiguration("zero_config").perform(context)
+    realsense_config = LaunchConfiguration("realsense_config").perform(context)
+    realsense_launch = (
+        Path(get_package_share_directory("realsense2_camera"))
+        / "launch"
+        / "rs_launch.py"
+    )
+
+    camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(str(realsense_launch)),
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        launch_arguments={
+            "camera_name": "camera",
+            "camera_namespace": "camera",
+            "serial_no": LaunchConfiguration("camera_serial_number"),
+            "config_file": realsense_config,
+            "enable_color": "true",
+            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
+            "depth_module.color_profile": LaunchConfiguration("color_profile"),
+            "enable_depth": "false",
+            "enable_sync": "false",
+            "align_depth.enable": "false",
+            "diagnostics_period": "1.0",
+        }.items(),
+    )
+
+    raw_topic = "/camera/camera/color/image_raw"
+    camera_info_topic = "/camera/camera/color/camera_info"
+    rect_topic = "/camera/camera/color/image_rect"
+    vision_container = ComposableNodeContainer(
+        name="g20_thumb_zero_vision_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name="rectify_color",
+                namespace="/camera/camera/color",
+                remappings=[
+                    ("image", raw_topic),
+                    ("camera_info", camera_info_topic),
+                    ("image_rect", rect_topic),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="apriltag_ros",
+                plugin="AprilTagNode",
+                name="apriltag",
+                namespace="/apriltag",
+                parameters=[
+                    tag_config,
+                    {
+                        "detector.decimate": ParameterValue(
+                            LaunchConfiguration("apriltag_decimate"),
+                            value_type=float,
+                        )
+                    },
+                ],
+                remappings=[
+                    ("image_rect", rect_topic),
+                    ("camera_info", camera_info_topic),
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    sdk = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="linker_hand_sdk",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("start_sdk")),
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "can": LaunchConfiguration("can_interface"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "move_on_startup": False,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"),
+                    value_type=int,
+                ),
+                "startup_torque": 80,
+                "state_poll_rate": 10.0,
+                "repeat_position_commands": False,
+                "is_touch": False,
+            }
+        ],
+    )
+
+    zero_node = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="cmc_pitch_zero_node",
+        name="g20_thumb_cmc_pitch_zero",
+        output="screen",
+        parameters=[
+            zero_config,
+            {
+                "serial_number": serial_number,
+                "session_dir": str(session_dir),
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"),
+                    value_type=bool,
+                ),
+                "image_topic": rect_topic,
+                "publish_debug_image": ParameterValue(
+                    LaunchConfiguration("publish_debug_image"),
+                    value_type=bool,
+                ),
+            },
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"G20 CMC pitch zero session: {session_dir}"),
+        LogInfo(
+            msg=(
+                "Only T0(ID 0) and T3(ID 1) are required; "
+                "T4/T5 detections are ignored"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Motor 0 performs three 255->64->255 sweeps; "
+                "zero angles come from the fitted T3-centre trajectory radius"
+            )
+        ),
+        camera,
+        vision_container,
+        sdk,
+        zero_node,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument("camera_serial_number", default_value=""),
+            DeclareLaunchArgument(
+                "color_profile", default_value="1280x720x30"
+            ),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument(
+                "publish_debug_image", default_value="true"
+            ),
+            DeclareLaunchArgument("start_camera", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "zero_config",
+                default_value=str(
+                    package_share / "config" / "cmc_pitch_zero.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "realsense_config",
+                default_value=str(
+                    package_share / "config" / "realsense_color_qos.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(
+                    package_share / "config" / "front_tags.yaml"
+                ),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py

@@ -0,0 +1,242 @@
+"""Launch front-camera trajectory-circle CMC roll zero/travel calibration."""
+
+from __future__ import annotations
+
+from datetime import datetime
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def _launch_stack(context):
+    serial_number = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not serial_number
+        or serial_number == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
+        or serial_number in {".", ".."}
+    ):
+        raise RuntimeError(
+            "serial_number is required and may contain only letters, "
+            "digits, dot, underscore and dash"
+        )
+
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(
+        LaunchConfiguration("output_root").perform(context)
+    ).expanduser().resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        session_dir = output_root / serial_number / timestamp
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    tag_config = LaunchConfiguration("tag_config").perform(context)
+    calibration_config = LaunchConfiguration(
+        "calibration_config"
+    ).perform(context)
+    realsense_config = LaunchConfiguration("realsense_config").perform(context)
+    realsense_launch = (
+        Path(get_package_share_directory("realsense2_camera"))
+        / "launch"
+        / "rs_launch.py"
+    )
+
+    camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(str(realsense_launch)),
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        launch_arguments={
+            "camera_name": "camera",
+            "camera_namespace": "camera",
+            "serial_no": LaunchConfiguration("camera_serial_number"),
+            "config_file": realsense_config,
+            "enable_color": "true",
+            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
+            "depth_module.color_profile": LaunchConfiguration("color_profile"),
+            "enable_depth": "false",
+            "enable_sync": "false",
+            "align_depth.enable": "false",
+            "diagnostics_period": "1.0",
+        }.items(),
+    )
+
+    raw_topic = "/camera/camera/color/image_raw"
+    camera_info_topic = "/camera/camera/color/camera_info"
+    rect_topic = "/camera/camera/color/image_rect"
+    vision_container = ComposableNodeContainer(
+        name="g20_thumb_roll_vision_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name="rectify_color",
+                namespace="/camera/camera/color",
+                remappings=[
+                    ("image", raw_topic),
+                    ("camera_info", camera_info_topic),
+                    ("image_rect", rect_topic),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="apriltag_ros",
+                plugin="AprilTagNode",
+                name="apriltag",
+                namespace="/apriltag",
+                parameters=[
+                    tag_config,
+                    {
+                        "detector.decimate": ParameterValue(
+                            LaunchConfiguration("apriltag_decimate"),
+                            value_type=float,
+                        )
+                    },
+                ],
+                remappings=[
+                    ("image_rect", rect_topic),
+                    ("camera_info", camera_info_topic),
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    sdk = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="linker_hand_sdk",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("start_sdk")),
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "can": LaunchConfiguration("can_interface"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "move_on_startup": False,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"),
+                    value_type=int,
+                ),
+                "startup_torque": 80,
+                "state_poll_rate": 10.0,
+                "repeat_position_commands": False,
+                "is_touch": False,
+            }
+        ],
+    )
+
+    calibration_node = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="cmc_roll_calibration_node",
+        name="g20_thumb_cmc_roll_calibration",
+        output="screen",
+        parameters=[
+            calibration_config,
+            {
+                "serial_number": serial_number,
+                "session_dir": str(session_dir),
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"),
+                    value_type=bool,
+                ),
+                "image_topic": rect_topic,
+                "publish_debug_image": ParameterValue(
+                    LaunchConfiguration("publish_debug_image"),
+                    value_type=bool,
+                ),
+            },
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"G20 CMC roll calibration session: {session_dir}"),
+        LogInfo(
+            msg=(
+                "Only T0(ID 0) and T3(ID 1) are required; "
+                "T4/T5 detections are ignored"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Motor 5 performs three 255->0->255 sweeps; "
+                "static captures measure both zero and angular travel"
+            )
+        ),
+        camera,
+        vision_container,
+        sdk,
+        calibration_node,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument("camera_serial_number", default_value=""),
+            DeclareLaunchArgument(
+                "color_profile", default_value="1280x720x30"
+            ),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument(
+                "publish_debug_image", default_value="true"
+            ),
+            DeclareLaunchArgument("start_camera", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(
+                    package_share
+                    / "config"
+                    / "cmc_roll_zero_travel.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "realsense_config",
+                default_value=str(
+                    package_share / "config" / "realsense_color_qos.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(
+                    package_share / "config" / "front_tags.yaml"
+                ),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py

@@ -0,0 +1,364 @@
+"""Launch the complete front-camera G20 thumb calibration stack."""
+
+from __future__ import annotations
+
+from datetime import datetime
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def _launch_stack(context):
+    serial_number = LaunchConfiguration("serial_number").perform(context)
+    if not serial_number or serial_number == "UNSET":
+        raise RuntimeError(
+            "serial_number is required, for example serial_number:=G20_LEFT_001"
+        )
+    if (
+        re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
+        or serial_number in {".", ".."}
+    ):
+        raise RuntimeError(
+            "serial_number may contain only letters, digits, dot, underscore and dash"
+        )
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        session_dir = output_root / serial_number / timestamp
+    session_dir.mkdir(parents=True, exist_ok=True)
+    bag_path = session_dir / "rosbag"
+
+    calibration_config = LaunchConfiguration("calibration_config").perform(context)
+    tag_config = LaunchConfiguration("tag_config").perform(context)
+    realsense_config = LaunchConfiguration("realsense_config").perform(context)
+    use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
+    if use_roi_text not in {"true", "false"}:
+        raise RuntimeError("use_roi must be true or false")
+    use_roi = use_roi_text == "true"
+
+    roi_values = {}
+    for name in ("roi_x", "roi_y", "roi_width", "roi_height"):
+        text = LaunchConfiguration(name).perform(context)
+        try:
+            roi_values[name] = int(text)
+        except ValueError as error:
+            raise RuntimeError(f"{name} must be an integer") from error
+    if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0:
+        raise RuntimeError("roi_x and roi_y must be non-negative")
+    if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
+        raise RuntimeError("roi_width and roi_height must be positive")
+
+    color_profile = LaunchConfiguration("color_profile").perform(context)
+    profile_match = re.fullmatch(r"(\d+)x(\d+)x(\d+)", color_profile)
+    if use_roi and profile_match is not None:
+        image_width = int(profile_match.group(1))
+        image_height = int(profile_match.group(2))
+        if (
+            roi_values["roi_x"] + roi_values["roi_width"] > image_width
+            or roi_values["roi_y"] + roi_values["roi_height"] > image_height
+        ):
+            raise RuntimeError(
+                "ROI lies outside color_profile "
+                f"{image_width}x{image_height}"
+            )
+
+    realsense_launch = Path(
+        get_package_share_directory("realsense2_camera")
+    ) / "launch" / "rs_launch.py"
+
+    camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(str(realsense_launch)),
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        launch_arguments={
+            "camera_name": "camera",
+            "camera_namespace": "camera",
+            "serial_no": LaunchConfiguration("camera_serial_number"),
+            "config_file": realsense_config,
+            "enable_color": "true",
+            # D405 exposes color from the stereo/depth module. Keep the RGB
+            # camera argument as well so the same launch file also works with
+            # D435/D455-class devices.
+            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
+            "depth_module.color_profile": LaunchConfiguration("color_profile"),
+            "enable_depth": LaunchConfiguration("enable_depth"),
+            "depth_module.depth_profile": LaunchConfiguration("depth_profile"),
+            # Depth is archival/diagnostic only. Synchronising and aligning it
+            # adds significant D405 processing latency without affecting the
+            # relative AprilTag rotation calculation.
+            "enable_sync": "false",
+            "align_depth.enable": "false",
+            "diagnostics_period": "1.0",
+        }.items(),
+    )
+
+    vision_components = []
+    if use_roi:
+        processed_image_raw_topic = "/g20_thumb_roi/image_raw"
+        processed_camera_info_topic = "/g20_thumb_roi/camera_info"
+        processed_image_rect_topic = "/g20_thumb_roi/image_rect"
+        vision_components.append(
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::CropDecimateNode",
+                name="crop_color_roi",
+                namespace="/g20_thumb_roi",
+                remappings=[
+                    ("in/image_raw", "/camera/camera/color/image_raw"),
+                    ("in/camera_info", "/camera/camera/color/camera_info"),
+                    ("out/image_raw", processed_image_raw_topic),
+                    ("out/camera_info", processed_camera_info_topic),
+                ],
+                parameters=[
+                    {
+                        "queue_size": 5,
+                        "decimation_x": 1,
+                        "decimation_y": 1,
+                        "offset_x": roi_values["roi_x"],
+                        "offset_y": roi_values["roi_y"],
+                        "width": roi_values["roi_width"],
+                        "height": roi_values["roi_height"],
+                    }
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            )
+        )
+        rectifier_namespace = "/g20_thumb_roi"
+        rectifier_name = "rectify_color_roi"
+    else:
+        processed_image_raw_topic = "/camera/camera/color/image_raw"
+        processed_camera_info_topic = "/camera/camera/color/camera_info"
+        processed_image_rect_topic = "/camera/camera/color/image_rect"
+        rectifier_namespace = "/camera/camera/color"
+        rectifier_name = "rectify_color"
+
+    vision_components.append(
+        ComposableNode(
+            package="image_proc",
+            plugin="image_proc::RectifyNode",
+            name=rectifier_name,
+            namespace=rectifier_namespace,
+            remappings=[
+                ("image", processed_image_raw_topic),
+                ("camera_info", processed_camera_info_topic),
+                ("image_rect", processed_image_rect_topic),
+            ],
+            parameters=[{"queue_size": 1}],
+            extra_arguments=[{"use_intra_process_comms": True}],
+        )
+    )
+
+    vision_components.append(
+        ComposableNode(
+            package="apriltag_ros",
+            plugin="AprilTagNode",
+            name="apriltag",
+            namespace="/apriltag",
+            parameters=[
+                tag_config,
+                {
+                    "detector.decimate": ParameterValue(
+                        LaunchConfiguration("apriltag_decimate"),
+                        value_type=float,
+                    )
+                },
+            ],
+            remappings=[
+                ("image_rect", processed_image_rect_topic),
+                ("camera_info", processed_camera_info_topic),
+            ],
+            extra_arguments=[{"use_intra_process_comms": True}],
+        )
+    )
+
+    vision_container = ComposableNodeContainer(
+        name="g20_thumb_vision_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=vision_components,
+        output="screen",
+        emulate_tty=True,
+    )
+
+    sdk = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="linker_hand_sdk",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("start_sdk")),
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "can": LaunchConfiguration("can_interface"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "move_on_startup": False,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"),
+                    value_type=int,
+                ),
+                "startup_torque": 80,
+                "state_poll_rate": 10.0,
+                "repeat_position_commands": False,
+                "is_touch": False,
+            }
+        ],
+    )
+
+    calibration = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="calibration_node",
+        name="g20_thumb_calibration",
+        output="screen",
+        parameters=[
+            calibration_config,
+            tag_config,
+            {
+                "serial_number": serial_number,
+                "session_dir": str(session_dir),
+                "commands_enabled": LaunchConfiguration("commands_enabled"),
+                "calibration_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"),
+                    value_type=int,
+                ),
+                "continuous_motion_mode": ParameterValue(
+                    LaunchConfiguration("continuous_motion_mode"),
+                    value_type=str,
+                ),
+                "angle_estimation_mode": ParameterValue(
+                    LaunchConfiguration("angle_estimation_mode"),
+                    value_type=str,
+                ),
+                "camera_serial_number": LaunchConfiguration("camera_serial_number"),
+                "rosbag_path": str(bag_path),
+                "camera_info_topic": processed_camera_info_topic,
+                "image_topic": processed_image_rect_topic,
+                "publish_debug_image": LaunchConfiguration(
+                    "publish_debug_image"
+                ),
+            },
+        ],
+    )
+
+    bag = ExecuteProcess(
+        condition=IfCondition(LaunchConfiguration("record_bag")),
+        cmd=[
+            "ros2",
+            "bag",
+            "record",
+            "--storage",
+            "mcap",
+            "--storage-preset-profile",
+            "zstd_fast",
+            "--max-bag-size",
+            "10737418240",
+            "--output",
+            str(bag_path),
+            processed_image_raw_topic,
+            processed_camera_info_topic,
+            "/camera/camera/depth/image_rect_raw",
+            "/apriltag/detections",
+            "/tf",
+            "/g20/cb_left_hand_control_cmd",
+            "/g20/cb_left_hand_state",
+            "/g20/cb_left_hand_info",
+            "/g20_thumb_calibration/status",
+        ],
+        output="screen",
+    )
+
+    actions = [
+        LogInfo(msg=f"G20 thumb calibration session: {session_dir}"),
+        LogInfo(
+            msg=(
+                "G20 thumb image ROI: "
+                f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, "
+                f"width={roi_values['roi_width']}, "
+                f"height={roi_values['roi_height']}"
+                if use_roi
+                else "G20 thumb image ROI: disabled"
+            )
+        ),
+        camera,
+        vision_container,
+    ]
+    actions.extend([sdk, calibration, bag])
+    return actions
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    default_output = str(Path.cwd() / "calibration_output")
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument("camera_serial_number", default_value=""),
+            DeclareLaunchArgument(
+                "color_profile", default_value="1280x720x30"
+            ),
+            DeclareLaunchArgument(
+                "depth_profile", default_value="640x480x30"
+            ),
+            DeclareLaunchArgument("enable_depth", default_value="false"),
+            DeclareLaunchArgument(
+                "publish_debug_image", default_value="false"
+            ),
+            DeclareLaunchArgument("use_roi", default_value="false"),
+            DeclareLaunchArgument("roi_x", default_value="128"),
+            DeclareLaunchArgument("roi_y", default_value="192"),
+            DeclareLaunchArgument("roi_width", default_value="1024"),
+            DeclareLaunchArgument("roi_height", default_value="528"),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument(
+                "continuous_motion_mode", default_value="endpoint"
+            ),
+            DeclareLaunchArgument(
+                "angle_estimation_mode",
+                default_value="trajectory_center_3d",
+            ),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument("start_camera", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument("record_bag", default_value="false"),
+            DeclareLaunchArgument("output_root", default_value=default_output),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(package_share / "config" / "calibration.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "realsense_config",
+                default_value=str(
+                    package_share / "config" / "realsense_color_qos.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(package_share / "config" / "front_tags.yaml"),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>g20_thumb_apriltag_calibration</name>
+  <version>0.1.0</version>
+  <description>Front RealSense AprilTag calibration for the left G20 thumb.</description>
+  <maintainer email="support@linker-robotics.com">lxp</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>apriltag_msgs</exec_depend>
+  <exec_depend>apriltag_ros</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>image_proc</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>linker_hand_ros2_sdk</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>realsense2_camera</exec_depend>
+  <exec_depend>rosbag2</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>tf2_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/g20_thumb_apriltag_calibration/pyproject.toml

@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61"]
+build-backend = "setuptools.build_meta"

src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration

@@ -0,0 +1 @@
+

src/g20_thumb_apriltag_calibration/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/g20_thumb_apriltag_calibration
+[install]
+install_scripts=$base/lib/g20_thumb_apriltag_calibration

src/g20_thumb_apriltag_calibration/setup.py

@@ -0,0 +1,41 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+
+package_name = "g20_thumb_apriltag_calibration"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=find_packages(),
+    data_files=[
+        (
+            "share/ament_index/resource_index/packages",
+            ["resource/" + package_name],
+        ),
+        ("share/" + package_name, ["package.xml", "README.md"]),
+        ("share/" + package_name + "/config", glob("config/*.yaml")),
+        ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
+    ],
+    install_requires=["setuptools", "numpy", "scipy"],
+    tests_require=["pytest"],
+    zip_safe=True,
+    maintainer="lxp",
+    maintainer_email="support@linker-robotics.com",
+    description="Front RealSense AprilTag calibration for the left G20 thumb",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            "calibration_node = g20_thumb_apriltag_calibration.node:main",
+            (
+                "cmc_pitch_zero_node = "
+                "g20_thumb_apriltag_calibration.zero_node:main"
+            ),
+            (
+                "cmc_roll_calibration_node = "
+                "g20_thumb_apriltag_calibration.zero_node:main"
+            ),
+        ],
+    },
+)

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

@@ -0,0 +1,383 @@
+from __future__ import annotations
+
+from dataclasses import replace
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.acquisition import (
+    ContinuousSweepCollector,
+    Observation,
+    PointCollector,
+    StateSample,
+    TagQuality,
+    aggregate_observations,
+    aggregate_sweep_observations,
+    interpolate_state_u8,
+    tag_quality_is_valid,
+)
+from g20_thumb_apriltag_calibration.core import PAIR_NAMES
+
+
+def _observation(index: int, angle_rad: float = 0.0) -> Observation:
+    quaternion = tuple(
+        float(value)
+        for value in Rotation.from_rotvec([0.0, angle_rad, 0.0]).as_quat()
+    )
+    return Observation(
+        stamp_ns=index,
+        received_at=index / 30.0,
+        relative_quaternion_xyzw={pair: quaternion for pair in PAIR_NAMES},
+        tag_quality={
+            role: TagQuality(hamming=0, decision_margin=50.0, edge_pixels=45.0)
+            for role in ("t0", "t3", "t4", "t5")
+        },
+        state_u8=tuple(float(value) for value in range(20)),
+    )
+
+
+def _point_observation(
+    index: int,
+    *,
+    angle_rad: float = 0.0,
+    motor_value: float = 100.0,
+) -> Observation:
+    state = [255.0] * 20
+    state[0] = float(motor_value)
+    return replace(
+        _observation(index, angle_rad),
+        state_u8=tuple(state),
+        state_stamp_ns=index,
+        state_sync_error_ns=5_000_000,
+    )
+
+
+def test_stable_window_then_thirty_frame_capture() -> None:
+    collector = PointCollector(
+        stable_frames=15,
+        capture_frames=30,
+        minimum_settle_seconds=0.4,
+        maximum_stable_spread_rad=np.deg2rad(0.3),
+    )
+    collector.start(0.0)
+    result = None
+    for index in range(15):
+        result = collector.add(_observation(index), index / 30.0)
+    assert result is None
+    assert collector.state == "capturing"
+    assert collector.stable_frames_seen == 15
+    assert collector.capture_frames_seen == 0
+    for index in range(15, 45):
+        result = collector.add(_observation(index), index / 30.0)
+    assert result is not None
+    assert collector.capture_frames_seen == 30
+    assert result["valid_frames"] == 30
+    assert len(result["state_u8_median"]) == 20
+
+
+def test_translation_stability_ignores_planar_orientation_jitter() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        stability_mode="translation",
+        maximum_stable_translation_spread_m=0.001,
+    )
+    collector.start(0.0)
+    result = None
+    for index in range(6):
+        observation = replace(
+            _observation(index, angle_rad=np.deg2rad(10.0 * index)),
+            tag_translation_xyz_m={
+                "t0": (0.00, 0.00, 0.50),
+                "t3": (0.03, 0.00, 0.50),
+                "t4": (0.06, 0.00, 0.50),
+                "t5": (0.09, 0.00, 0.50),
+            },
+        )
+        result = collector.add(observation, index / 30.0)
+    assert result is not None
+    assert collector.state == "complete"
+    assert collector.stable_spread_rad == {}
+    assert max(collector.stable_spread_m.values()) == 0.0
+
+
+def test_point_capture_waits_for_synchronised_target_state() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        maximum_stable_spread_rad=np.deg2rad(0.3),
+    )
+    collector.start(
+        0.0,
+        required_state_index=0,
+        required_state_u8=100.0,
+        maximum_state_error_u8=2.0,
+    )
+    for index in range(3):
+        collector.add(
+            _point_observation(index, motor_value=108.0),
+            index / 30.0,
+        )
+    assert collector.state == "settling"
+    assert collector.stable_frames_seen == 0
+    assert collector.reason == "motor_position_out_of_tolerance"
+
+    result = None
+    for index in range(3, 9):
+        result = collector.add(
+            _point_observation(index, motor_value=101.0),
+            index / 30.0,
+        )
+    assert result is not None
+    assert collector.state == "complete"
+    assert result["state_u8_median"][0] == 101.0
+
+
+def test_unstable_capture_frames_are_not_aggregated() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        maximum_stable_spread_rad=np.deg2rad(0.3),
+    )
+    collector.start(0.0)
+    for index in range(3):
+        collector.add(_observation(index), index / 30.0)
+    assert collector.state == "capturing"
+
+    result = None
+    # The capture block is internally stable but belongs to a different
+    # planar-PnP branch than the preceding stable window.
+    for index, angle_deg in enumerate((25.0, 25.0, 25.0), start=3):
+        result = collector.add(
+            _observation(index, np.deg2rad(angle_deg)),
+            index / 30.0,
+        )
+    assert result is None
+    assert collector.state == "settling"
+    assert collector.capture_frames_seen == 0
+    assert collector.reason.endswith("capture_not_stable")
+
+
+def test_position_drift_during_capture_restarts_settling() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+    )
+    collector.start(
+        0.0,
+        required_state_index=0,
+        required_state_u8=100.0,
+        maximum_state_error_u8=2.0,
+    )
+    for index in range(3):
+        collector.add(
+            _point_observation(index, motor_value=100.0),
+            index / 30.0,
+        )
+    assert collector.state == "capturing"
+    collector.add(
+        _point_observation(3, motor_value=108.0),
+        0.1,
+    )
+    assert collector.state == "settling"
+    assert collector.capture_frames_seen == 0
+    assert collector.reason == "motor_position_out_of_tolerance"
+
+
+def test_missing_tags_eventually_pauses_point_collector() -> None:
+    collector = PointCollector(settle_timeout_seconds=5.0)
+    collector.start(10.0)
+    collector.poll(15.01)
+    assert collector.state == "failed"
+    assert collector.reason == "settle_timeout"
+
+
+def test_unstable_window_does_not_enter_capture() -> None:
+    collector = PointCollector(maximum_stable_spread_rad=np.deg2rad(0.3))
+    collector.start(0.0)
+    for index in range(15):
+        angle = np.deg2rad(1.0 if index % 2 else -1.0)
+        collector.add(_observation(index, angle), index / 30.0)
+    assert collector.state == "settling"
+    assert collector.reason.endswith("not_stable")
+    assert max(collector.stable_spread_rad.values()) > np.deg2rad(0.3)
+
+
+def test_isolated_invalid_frame_is_skipped_without_losing_valid_window() -> None:
+    collector = PointCollector()
+    collector.start(0.0)
+    for index in range(14):
+        collector.add(_observation(index), index / 30.0)
+    collector.mark_invalid_frame()
+    assert collector.stable_frames_seen == 14
+    collector.add(_observation(15), 0.5)
+    assert collector.state == "capturing"
+    assert collector.reason == ""
+
+
+def test_three_consecutive_invalid_frames_clear_stability_window() -> None:
+    collector = PointCollector()
+    collector.start(0.0)
+    for index in range(14):
+        collector.add(_observation(index), index / 30.0)
+    for _ in range(3):
+        collector.mark_invalid_frame()
+    assert collector.stable_frames_seen == 0
+    collector.add(_observation(15), 0.5)
+    assert collector.state == "settling"
+
+
+def test_bad_tag_quality_is_filtered() -> None:
+    good = TagQuality(hamming=0, decision_margin=31.0, edge_pixels=40.0)
+    bad_hamming = TagQuality(hamming=1, decision_margin=50.0, edge_pixels=50.0)
+    thresholds = {
+        "maximum_hamming": 0,
+        "minimum_decision_margin": 30.0,
+        "minimum_edge_pixels": 40.0,
+    }
+    assert tag_quality_is_valid(good, **thresholds)
+    assert not tag_quality_is_valid(bad_hamming, **thresholds)
+
+
+def test_aggregate_keeps_worst_tag_quality() -> None:
+    observations = [_observation(0), _observation(1)]
+    aggregate = aggregate_observations(observations)
+    assert aggregate["tag_quality"]["t0"] == {
+        "minimum_decision_margin": 50.0,
+        "minimum_edge_pixels": 45.0,
+        "maximum_hamming": 0,
+    }
+
+
+def test_aggregate_keeps_median_tag_centres() -> None:
+    observations = [
+        replace(
+            _observation(index),
+            tag_translation_xyz_m={
+                role: (0.01 * index, 0.02, 0.50)
+                for role in ("t0", "t3", "t4", "t5")
+            },
+        )
+        for index in range(3)
+    ]
+    aggregate = aggregate_observations(observations)
+    assert aggregate["tag_translation_xyz_m"]["t4"] == pytest.approx(
+        [0.01, 0.02, 0.50]
+    )
+
+
+def test_pnp_reprojection_error_is_filtered_and_aggregated() -> None:
+    good = TagQuality(
+        hamming=0,
+        decision_margin=50.0,
+        edge_pixels=40.0,
+        reprojection_error_px=0.4,
+    )
+    bad = replace(good, reprojection_error_px=1.6)
+    thresholds = {
+        "maximum_hamming": 0,
+        "minimum_decision_margin": 30.0,
+        "minimum_edge_pixels": 30.0,
+        "maximum_reprojection_error_px": 1.5,
+    }
+    assert tag_quality_is_valid(good, **thresholds)
+    assert not tag_quality_is_valid(bad, **thresholds)
+
+    observations = [
+        replace(
+            _observation(index),
+            tag_quality={
+                role: replace(good, reprojection_error_px=error)
+                for role in ("t0", "t3", "t4", "t5")
+            },
+        )
+        for index, error in enumerate((0.2, 0.7))
+    ]
+    aggregate = aggregate_observations(observations)
+    assert (
+        aggregate["tag_quality"]["t0"][
+            "maximum_reprojection_error_px"
+        ]
+        == 0.7
+    )
+
+
+def test_state_is_interpolated_at_camera_timestamp() -> None:
+    before = tuple([255.0] + [0.0] * 19)
+    after = tuple([235.0] + [0.0] * 19)
+    samples = [
+        StateSample(stamp_ns=1_000_000_000, position_u8=before),
+        StateSample(stamp_ns=1_100_000_000, position_u8=after),
+    ]
+    matched = interpolate_state_u8(
+        samples,
+        1_025_000_000,
+        maximum_skew_ns=60_000_000,
+    )
+    assert matched is not None
+    state, skew = matched
+    assert state[0] == 250.0
+    assert skew == 25_000_000
+    assert (
+        interpolate_state_u8(
+            samples,
+            1_300_000_000,
+            maximum_skew_ns=60_000_000,
+        )
+        is None
+    )
+
+
+def _synchronised_observation(
+    index: int, motor_value: float
+) -> Observation:
+    state = [255.0] * 20
+    state[0] = motor_value
+    return replace(
+        _observation(index),
+        state_u8=tuple(state),
+        state_stamp_ns=index,
+        state_sync_error_ns=5_000_000,
+    )
+
+
+def test_continuous_sweep_completes_after_full_span_and_endpoint_hold() -> None:
+    collector = ContinuousSweepCollector(
+        endpoint_hold_seconds=0.2,
+        minimum_valid_frames=20,
+        minimum_state_span_u8=240.0,
+    )
+    collector.start(0.0, motor_index=0, start_u8=255, target_u8=0)
+    result = None
+    for index, value in enumerate(np.linspace(255.0, 0.0, 60)):
+        result = collector.add(
+            _synchronised_observation(index, float(value)),
+            index * 0.05,
+        )
+    assert result is None
+    for offset in range(1, 6):
+        result = collector.add(
+            _synchronised_observation(60 + offset, 0.0),
+            3.0 + offset * 0.05,
+        )
+        if result is not None:
+            break
+    assert result is not None
+    assert collector.state == "complete"
+    assert collector.state_span_u8 == 255.0
+
+    bins = aggregate_sweep_observations(
+        result,
+        motor_index=0,
+        start_u8=255,
+        target_u8=0,
+        endpoint_tolerance_u8=2.0,
+    )
+    assert 0 in bins
+    assert 255 in bins
+    assert len(bins) >= 50

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -0,0 +1,184 @@
+from pathlib import Path
+
+import yaml
+
+
+PACKAGE_ROOT = Path(__file__).resolve().parents[1]
+
+
+def test_front_tag_parameters_match_namespaced_detector() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
+    )
+
+    detector = config["/apriltag/apriltag"]["ros__parameters"]
+    calibration = config["g20_thumb_calibration"]["ros__parameters"]
+
+    assert detector["tag"]["ids"] == calibration["tag_ids"]
+    assert detector["tag"]["frames"] == calibration["tag_frames"]
+    assert detector["tag"]["sizes"] == calibration["tag_sizes_m"]
+    assert detector["tag"]["ids"] == [0, 1, 2, 3]
+    assert detector["qos_profile"] == "sensor_data"
+    assert detector["detector"]["decimate"] == 1.5
+    assert detector["detector"]["refine"] is True
+    assert detector["detector"]["debug"] is False
+
+
+def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
+    )
+    parameters = config["g20_thumb_calibration"]["ros__parameters"]
+
+    assert parameters["angle_estimation_mode"] == "trajectory_center_3d"
+    assert parameters["passive_ip_multiplier"] == 1.02
+    assert parameters["pnp_minimum_valid_rate"] == 0.95
+    assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5
+    assert parameters["pnp_reprojection_tie_px"] == 1.5
+    assert parameters["pnp_tracker_reset_seconds"] == 5.0
+    assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0
+    assert parameters["pnp_group_relative_translation_scale_m"] == 0.01
+    assert parameters["pnp_group_reprojection_weight"] == 0.05
+    assert parameters["pnp_trajectory_reprojection_scale_px"] == 0.1
+    assert parameters["pnp_rigid_rotation_scale_deg"] == 5.0
+    assert parameters["pnp_rigid_p95_accepted_drift_deg"] == 8.0
+    assert parameters["pnp_rigid_maximum_accepted_drift_deg"] == 15.0
+    assert (
+        parameters["pnp_rigid_p95_accepted_distance_drift_m"]
+        <= 0.003
+    )
+    assert (
+        parameters["pnp_rigid_maximum_accepted_distance_drift_m"]
+        <= 0.006
+    )
+    assert parameters["pnp_maximum_pose_jump_deg"] <= 35.0
+    assert parameters["trajectory_maximum_plane_rms_m"] <= 0.004
+    assert parameters["trajectory_maximum_radial_rms_m"] <= 0.004
+    assert parameters["trajectory_minimum_radius_m"] >= 0.005
+    assert parameters["trajectory_minimum_arc_deg"] >= 15.0
+    assert (
+        parameters["trajectory_maximum_root_role_disagreement_deg"]
+        <= 5.0
+    )
+    assert parameters["trajectory_maximum_anchor_drift_m"] <= 0.005
+    assert parameters["trajectory_static_translation_outlier_m"] <= 0.005
+    assert (
+        parameters["trajectory_maximum_static_translation_rms_m"]
+        <= 0.002
+    )
+    assert parameters["minimum_edge_pixels"] == 30.0
+    assert parameters["maximum_static_std_deg"] == 3.0
+    assert parameters["minimum_pose_inlier_rate"] == 0.90
+    assert parameters["repetitions"] == 1
+    assert parameters["command_step"] == 8
+    assert parameters["scan_mode"] == "continuous"
+    assert parameters["continuous_motion_mode"] == "endpoint"
+    assert parameters["auto_start_tip"] is True
+    assert parameters["minimum_detection_hz"] == 15.0
+    assert parameters["stable_frames"] == 5
+    assert parameters["capture_frames"] == 8
+    assert parameters["maximum_stable_spread_deg"] == 3.0
+    assert parameters["maximum_stable_translation_spread_m"] <= 0.003
+    assert parameters["validation_command_count"] == 5
+    assert parameters["continuous_minimum_bins"] >= 32
+    assert parameters["continuous_maximum_bin_gap"] <= 16
+    assert parameters["continuous_segment_minimum_seconds"] >= 0.1
+    assert parameters["continuous_segment_timeout_seconds"] >= 5.0
+    assert parameters["continuous_prepare_timeout_seconds"] >= 30.0
+
+
+def test_realsense_color_and_camera_info_use_matching_qos() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "realsense_color_qos.yaml").read_text()
+    )
+
+    assert config["color_qos"] == "DEFAULT"
+    assert config["color_info_qos"] == "DEFAULT"
+
+
+def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text()
+    )
+    parameters = config["g20_thumb_cmc_pitch_zero"]["ros__parameters"]
+
+    assert parameters["t0_id"] == 0
+    assert parameters["t3_id"] == 1
+    assert parameters["joint_name"] == "thumb_cmc_pitch"
+    assert parameters["motor_index"] == 0
+    assert parameters["zero_command_u8"] == 255
+    assert parameters["measure_travel"] is False
+    assert parameters["baseline_command_u8"] == [
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        193,
+        148,
+        105,
+        42,
+        245,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+    ]
+    assert parameters["repetitions"] == 3
+    assert parameters["zero_capture_frames"] == 30
+    assert parameters["trajectory_command_u8"] <= 64
+    assert parameters["trajectory_minimum_state_span_u8"] >= 160.0
+    assert parameters["trajectory_minimum_bins"] >= 18
+    assert parameters["trajectory_minimum_arc_deg"] >= 20.0
+    assert parameters["trajectory_maximum_radial_rms_px"] <= 2.0
+    assert parameters["trajectory_maximum_p95_radial_error_px"] <= 3.5
+    assert parameters["minimum_detection_rate"] == 0.95
+    assert parameters["minimum_edge_pixels"] == 40.0
+    assert parameters["maximum_static_position_rms_px"] <= 1.5
+    assert "maximum_axis_alignment_deg" not in parameters
+    assert "maximum_static_std_deg" not in parameters
+    assert "maximum_anchor_drift_deg" not in parameters
+    assert parameters["camera_alignment_enabled"] is True
+    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
+    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
+    assert parameters["camera_alignment_required_frames"] >= 10
+    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8
+
+
+def test_cmc_roll_config_measures_full_endpoint_travel() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "cmc_roll_zero_travel.yaml").read_text()
+    )
+    parameters = config[
+        "g20_thumb_cmc_roll_calibration"
+    ]["ros__parameters"]
+
+    assert parameters["t0_id"] == 0
+    assert parameters["t3_id"] == 1
+    assert parameters["joint_name"] == "thumb_cmc_roll"
+    assert parameters["motor_index"] == 5
+    assert parameters["zero_command_u8"] == 255
+    assert parameters["trajectory_command_u8"] == 0
+    assert parameters["measure_travel"] is True
+    assert parameters["baseline_command_u8"][5] == 255
+    assert parameters["repetitions"] == 3
+    assert parameters["zero_capture_frames"] == 30
+    assert parameters["trajectory_minimum_state_span_u8"] >= 240.0
+    assert parameters["trajectory_minimum_bins"] >= 30
+    assert parameters["minimum_travel_deg"] >= 20.0
+    assert parameters["maximum_travel_difference_deg"] <= 1.0
+    assert parameters["maximum_round_difference_deg"] == 1.0
+    assert "maximum_axis_alignment_deg" not in parameters
+    assert "maximum_static_std_deg" not in parameters
+    assert "maximum_anchor_drift_deg" not in parameters
+    assert parameters["camera_alignment_enabled"] is True
+    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
+    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
+    assert parameters["camera_alignment_required_frames"] >= 10
+    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8

src/g20_thumb_apriltag_calibration/test/test_core.py

@@ -0,0 +1,244 @@
+from __future__ import annotations
+
+import copy
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.core import (
+    BASELINE_COMMAND,
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+    PAIR_IP,
+    PAIR_MCP,
+    PAIR_ROOT,
+    PHASE_ROOT,
+    PHASE_TIP,
+    build_command,
+    create_final_payload,
+    fit_calibration_curves,
+    image_plane_tag_quaternion_xyzw,
+    isotonic_nonincreasing,
+    relative_quaternion_xyzw,
+    rotation_inlier_fraction,
+    rotation_rms_rad,
+    scan_targets,
+    validate_final_payload,
+)
+
+
+def _quaternion(base: Rotation, axis: np.ndarray, angle: float) -> list[float]:
+    value = base * Rotation.from_rotvec(axis * angle)
+    return [float(component) for component in value.as_quat()]
+
+
+def _synthetic_records(
+    repetitions: int = 3,
+    command_step: int = 1,
+) -> list[dict]:
+    bases = {
+        PAIR_ROOT: Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
+        PAIR_MCP: Rotation.from_euler("xyz", [-0.15, 0.1, 0.25]),
+        PAIR_IP: Rotation.from_euler("xyz", [0.05, 0.2, -0.2]),
+    }
+    axes = {
+        PAIR_ROOT: np.asarray([0.2, 0.9, -0.1], dtype=float),
+        PAIR_MCP: np.asarray([-0.1, 0.3, 0.95], dtype=float),
+        PAIR_IP: np.asarray([0.05, -0.2, 0.98], dtype=float),
+    }
+    axes = {key: value / np.linalg.norm(value) for key, value in axes.items()}
+    records: list[dict] = []
+    for phase in (PHASE_ROOT, PHASE_TIP):
+        for cycle, direction, command in scan_targets(
+            repetitions, command_step
+        ):
+            progress = (255 - command) / 255.0
+            branch = (
+                0.008 * np.sin(np.pi * progress)
+                if direction == DIRECTION_INCREASING
+                else 0.0
+            )
+            root = 0.82 * progress + branch if phase == PHASE_ROOT else 0.0
+            mcp = 1.16 * progress + branch if phase == PHASE_TIP else 0.0
+            ip = 1.018 * mcp + 0.001 * np.sin(np.pi * progress)
+            rotations = {
+                PAIR_ROOT: _quaternion(bases[PAIR_ROOT], axes[PAIR_ROOT], root),
+                PAIR_MCP: _quaternion(bases[PAIR_MCP], axes[PAIR_MCP], mcp),
+                PAIR_IP: _quaternion(bases[PAIR_IP], axes[PAIR_IP], ip),
+            }
+            records.append(
+                {
+                    "kind": "sample",
+                    "phase": phase,
+                    "cycle": cycle,
+                    "direction": direction,
+                    "command_u8": command,
+                    "relative_quaternion_xyzw": rotations,
+                }
+            )
+    return records
+
+
+def test_build_command_changes_only_selected_channel() -> None:
+    result = build_command(15, 37)
+    assert len(result) == 20
+    assert result[15] == 37
+    assert result[:15] == list(BASELINE_COMMAND[:15])
+    assert result[16:] == list(BASELINE_COMMAND[16:])
+    roll_result = build_command(5, 20)
+    assert roll_result[5] == 20
+    assert roll_result[:5] == list(BASELINE_COMMAND[:5])
+    assert roll_result[6:] == list(BASELINE_COMMAND[6:])
+    with pytest.raises(ValueError):
+        build_command(6, 10)
+
+
+def test_three_cycle_scan_has_every_integer_in_both_directions() -> None:
+    targets = scan_targets(3, command_step=1)
+    assert len(targets) == 3 * 2 * 256
+    assert targets[0] == (0, DIRECTION_DECREASING, 255)
+    assert targets[255] == (0, DIRECTION_DECREASING, 0)
+    assert targets[256] == (0, DIRECTION_INCREASING, 0)
+    assert targets[511] == (0, DIRECTION_INCREASING, 255)
+
+
+def test_quick_scan_has_bounded_sparse_grid_and_endpoints() -> None:
+    targets = scan_targets(1, command_step=8)
+    assert len(targets) == 66
+    assert targets[0] == (0, DIRECTION_DECREASING, 255)
+    assert targets[32] == (0, DIRECTION_DECREASING, 0)
+    assert targets[33] == (0, DIRECTION_INCREASING, 0)
+    assert targets[-1] == (0, DIRECTION_INCREASING, 255)
+    increasing = [
+        command
+        for _, direction, command in targets
+        if direction == DIRECTION_INCREASING
+    ]
+    assert max(np.diff(increasing)) == 8
+
+
+def test_relative_rotation_cancels_camera_orientation() -> None:
+    camera_to_parent = Rotation.from_euler("xyz", [0.4, -0.2, 0.1])
+    parent_to_child = Rotation.from_rotvec([0.1, 0.3, -0.2])
+    camera_to_child = camera_to_parent * parent_to_child
+    actual = Rotation.from_quat(
+        relative_quaternion_xyzw(
+            camera_to_parent.as_quat(), camera_to_child.as_quat()
+        )
+    )
+    assert (parent_to_child.inv() * actual).magnitude() < 1e-10
+
+
+def test_image_plane_tag_rotation_uses_ordered_opposite_edges() -> None:
+    angle = np.deg2rad(27.0)
+    x_axis_image = np.asarray([np.cos(angle), -np.sin(angle)])
+    y_axis_image = np.asarray([np.sin(angle), np.cos(angle)])
+    corners = np.asarray(
+        [
+            -x_axis_image - y_axis_image,
+            x_axis_image - y_axis_image,
+            1.1 * x_axis_image + y_axis_image,
+            -0.9 * x_axis_image + y_axis_image,
+        ]
+    )
+    actual = Rotation.from_quat(image_plane_tag_quaternion_xyzw(corners))
+    expected = Rotation.from_rotvec([0.0, 0.0, angle])
+    assert (expected.inv() * actual).magnitude() < 1e-10
+
+
+def test_static_rms_rejects_isolated_planar_pnp_flip() -> None:
+    quaternions = [
+        Rotation.from_rotvec([0.0, np.deg2rad(0.1 * np.sin(index)), 0.0]).as_quat()
+        for index in range(149)
+    ]
+    quaternions.append(
+        Rotation.from_rotvec([0.0, np.deg2rad(27.0), 0.0]).as_quat()
+    )
+
+    threshold = np.deg2rad(5.0)
+    assert rotation_rms_rad(quaternions) > np.deg2rad(2.0)
+    assert rotation_rms_rad(
+        quaternions, outlier_threshold_rad=threshold
+    ) < np.deg2rad(0.5)
+    assert rotation_inlier_fraction(
+        quaternions, outlier_threshold_rad=threshold
+    ) == pytest.approx(149 / 150)
+
+
+def test_isotonic_projection_is_nonincreasing() -> None:
+    projected = isotonic_nonincreasing([3.0, 2.0, 2.4, 1.0, 0.0])
+    assert np.all(np.diff(projected) <= 0.0)
+    assert projected.tolist() == pytest.approx([3.0, 2.2, 2.2, 1.0, 0.0])
+
+
+def test_fit_produces_complete_runtime_payload() -> None:
+    fit = fit_calibration_curves(_synthetic_records())
+    assert fit.joints["thumb_cmc_pitch"]["decreasing_rad"][0] == pytest.approx(
+        0.82, abs=2e-3
+    )
+    assert fit.joints["thumb_mcp"]["decreasing_rad"][0] == pytest.approx(
+        1.16, abs=2e-3
+    )
+    assert fit.joints["thumb_ip"]["passive"] is True
+    assert fit.ip_coupling["r_squared"] > 0.999
+    for joint in fit.joints.values():
+        assert len(joint["angle_rad"]) == 256
+        assert len(joint["decreasing_rad"]) == 256
+        assert len(joint["increasing_rad"]) == 256
+        assert joint["angle_rad"][255] == 0.0
+        assert joint["decreasing_rad"][255] == 0.0
+        assert joint["increasing_rad"][255] == 0.0
+
+    payload = create_final_payload(
+        serial_number="G20_LEFT_TEST",
+        fit=fit,
+        validation_errors_rad=[0.01, -0.02],
+        passed=True,
+    )
+    assert set(payload) == {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "angle_unit",
+        "command_range",
+        "zero_command_u8",
+        "baseline_command_u8",
+        "joints",
+        "ip_coupling",
+        "quality",
+    }
+    assert payload["schema_version"] == 2
+    for joint in payload["joints"].values():
+        assert "angle_rad" in joint
+        assert "decreasing_rad" not in joint
+        assert "increasing_rad" not in joint
+    validate_final_payload(payload)
+
+    invalid = copy.deepcopy(payload)
+    invalid["joints"]["thumb_mcp"]["angle_rad"].pop()
+    with pytest.raises(ValueError):
+        validate_final_payload(invalid)
+
+
+def test_sparse_fit_interpolates_complete_monotonic_runtime_payload() -> None:
+    fit = fit_calibration_curves(
+        _synthetic_records(repetitions=1, command_step=8)
+    )
+    for joint in fit.joints.values():
+        combined = np.asarray(joint["angle_rad"], dtype=float)
+        assert combined.shape == (256,)
+        assert combined[255] == 0.0
+        assert np.all(np.diff(combined) <= 1e-10)
+        for direction in ("decreasing_rad", "increasing_rad"):
+            curve = np.asarray(joint[direction], dtype=float)
+            assert curve.shape == (256,)
+            assert curve[255] == 0.0
+            assert np.all(np.diff(curve) <= 1e-10)
+    assert fit.joints["thumb_cmc_pitch"]["decreasing_rad"][0] == pytest.approx(
+        0.82, abs=2e-3
+    )
+    assert fit.joints["thumb_mcp"]["decreasing_rad"][0] == pytest.approx(
+        1.16, abs=2e-3
+    )

src/g20_thumb_apriltag_calibration/test/test_diagnostics.py

@@ -0,0 +1,105 @@
+from g20_thumb_apriltag_calibration.acquisition import TagQuality
+from g20_thumb_apriltag_calibration.diagnostics import (
+    build_tag_quality_diagnostics,
+    render_status_text_zh,
+    status_guidance_zh,
+)
+
+
+def _tag_config() -> dict[str, dict]:
+    return {
+        role: {"id": tag_id, "frame": role, "size_m": 0.01}
+        for role, tag_id in zip(("t0", "t3", "t4", "t5"), range(4))
+    }
+
+
+def test_invalid_t0_edge_has_specific_chinese_guidance() -> None:
+    qualities = {
+        role: TagQuality(
+            hamming=0,
+            decision_margin=100.0,
+            edge_pixels=29.4 if role == "t0" else 34.0,
+        )
+        for role in _tag_config()
+    }
+    diagnostics = build_tag_quality_diagnostics(
+        _tag_config(),
+        qualities,
+        {"t0": "tag_quality_invalid"},
+        {},
+        maximum_hamming=0,
+        minimum_decision_margin=30.0,
+        minimum_edge_pixels=30.0,
+        maximum_reprojection_error_px=1.5,
+    )
+
+    assert diagnostics["t0"]["individual_valid"] is False
+    assert diagnostics["t0"]["edge_pixels"] == 29.4
+    assert "边长29.4px" in diagnostics["t0"]["summary_zh"]
+    assert diagnostics["t3"]["individual_valid"] is True
+
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "point_capture_failed:invalid_tag_frame",
+        diagnostics,
+    )
+    assert "掌心T0" in reason_zh
+    assert "边长29.4px" in reason_zh
+    assert "相机稍微靠近" in action_zh
+    assert "resume" in action_zh
+    text = render_status_text_zh(
+        "标定已暂停",
+        reason_zh,
+        action_zh,
+        diagnostics,
+    )
+    assert "状态：标定已暂停" in text
+    assert "掌心T0(ID 0)：异常，边长29.4px" in text
+    assert "拇指根部T3(ID 1)：正常" in text
+
+
+def test_missing_tag_is_reported_without_manual_topic_parsing() -> None:
+    diagnostics = build_tag_quality_diagnostics(
+        _tag_config(),
+        {},
+        {"t5": "tag_not_detected"},
+        {},
+        maximum_hamming=0,
+        minimum_decision_margin=30.0,
+        minimum_edge_pixels=30.0,
+        maximum_reprojection_error_px=1.5,
+    )
+
+    assert diagnostics["t5"]["detected"] is False
+    assert "未检测到" in diagnostics["t5"]["summary_zh"]
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "point_capture_failed:invalid_tag_frame",
+        diagnostics,
+    )
+    assert "拇指末节T5" in reason_zh
+    assert "四张标签同时可见" in action_zh
+
+
+def test_low_detection_frequency_has_direct_chinese_action() -> None:
+    reason_zh, action_zh = status_guidance_zh(
+        "PREFLIGHT",
+        "detection_hz_too_low:11.29",
+        {},
+    )
+
+    assert "11.29Hz" in reason_zh
+    assert "额外订阅" in action_zh
+
+
+def test_synchronised_timeout_explains_resume_not_start() -> None:
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "continuous_sweep_failed:synchronised_tag_state_timeout",
+        {},
+    )
+
+    assert "运动过程中连续3秒" in reason_zh
+    assert "当前画面恢复正常" in reason_zh
+    assert "resume" in action_zh
+    assert "不要调用start" in action_zh

src/g20_thumb_apriltag_calibration/test/test_pnp.py

@@ -0,0 +1,542 @@
+from __future__ import annotations
+
+import cv2
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.pnp import (
+    SquareTagGroupPoseTracker,
+    SquareTagPose,
+    SquareTagPoseTracker,
+    rotation_distance_rad,
+    select_continuous_pose,
+    select_rigid_group_trajectory,
+    solve_square_tag_ippe,
+    square_object_points,
+)
+
+
+def _camera_matrix() -> np.ndarray:
+    return np.asarray(
+        [
+            [650.0, 0.0, 640.0],
+            [0.0, 650.0, 360.0],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+
+def _project(
+    rotation: Rotation,
+    translation_xyz_m: np.ndarray,
+    *,
+    tag_size_m: float = 0.01,
+) -> np.ndarray:
+    rotation_vector, _ = cv2.Rodrigues(rotation.as_matrix())
+    corners, _ = cv2.projectPoints(
+        square_object_points(tag_size_m),
+        rotation_vector,
+        translation_xyz_m,
+        _camera_matrix(),
+        np.zeros((4, 1)),
+    )
+    return corners.reshape(4, 2)
+
+
+def test_ippe_recovers_known_square_tag_pose() -> None:
+    expected_rotation = Rotation.from_euler(
+        "xyz", [10.0, -15.0, 25.0], degrees=True
+    )
+    expected_translation = np.asarray([0.02, -0.01, 0.25])
+    candidates = solve_square_tag_ippe(
+        _project(expected_rotation, expected_translation),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+    )
+
+    assert len(candidates) == 2
+    actual = min(candidates, key=lambda item: item.reprojection_error_px)
+    assert (
+        rotation_distance_rad(
+            expected_rotation.as_quat(),
+            actual.quaternion_xyzw,
+        )
+        < 1.0e-8
+    )
+    assert np.allclose(actual.translation_xyz_m, expected_translation)
+    assert actual.reprojection_error_px < 1.0e-8
+
+
+def test_temporal_selection_breaks_near_reprojection_tie() -> None:
+    previous = SquareTagPose(
+        quaternion_xyzw=(0.0, 0.0, 0.0, 1.0),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.2,
+    )
+    continuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.11,
+    )
+    flipped = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.1,
+    )
+
+    selected, reason = select_continuous_pose(
+        [flipped, continuous],
+        previous=previous,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=0.03,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == continuous
+
+
+def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None:
+    stale_mirror = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.25,
+    )
+    true_pose = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.05,
+    )
+    continued_mirror = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 54.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.24,
+    )
+
+    selected, reason = select_continuous_pose(
+        [continued_mirror, true_pose],
+        previous=stale_mirror,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=0.03,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == true_pose
+
+
+def test_active_motion_can_prioritise_continuous_branch() -> None:
+    previous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.25,
+    )
+    continuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 54.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.24,
+    )
+    discontinuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.05,
+    )
+
+    selected, reason = select_continuous_pose(
+        [continuous, discontinuous],
+        previous=previous,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=1.5,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == continuous
+
+
+def test_tracker_recovers_after_timestamp_gap() -> None:
+    tracker = SquareTagPoseTracker(
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=1.5,
+        maximum_pose_jump_rad=np.deg2rad(5.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+        reset_after_seconds=0.5,
+    )
+    first_rotation = Rotation.from_euler("y", 0.0, degrees=True)
+    second_rotation = Rotation.from_euler("y", 20.0, degrees=True)
+    first, first_reason = tracker.estimate(
+        "t0",
+        _project(first_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_000_000_000,
+    )
+    rejected, rejection_reason = tracker.estimate(
+        "t0",
+        _project(second_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_100_000_000,
+    )
+    recovered, recovered_reason = tracker.estimate(
+        "t0",
+        _project(second_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_700_000_000,
+    )
+
+    assert first is not None
+    assert first_reason == ""
+    assert rejected is None
+    assert rejection_reason == "pose_jump"
+    assert recovered is not None
+    assert recovered_reason == ""
+
+
+def _pose(
+    rotation_deg: float,
+    x_m: float,
+    reprojection_error_px: float,
+) -> SquareTagPose:
+    return SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler(
+                "y", rotation_deg, degrees=True
+            ).as_quat()
+        ),
+        translation_xyz_m=(x_m, 0.0, 0.25),
+        reprojection_error_px=reprojection_error_px,
+    )
+
+
+def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("t0", "t3", "t4", "t5"),
+        adjacent_pairs=(
+            ("t0", "t3"),
+            ("t3", "t4"),
+            ("t4", "t5"),
+        ),
+        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,
+    )
+    first = {
+        "t0": (_pose(0.0, 0.00, 0.05),),
+        "t3": (_pose(5.0, 0.03, 0.05),),
+        "t4": (_pose(15.0, 0.06, 0.05),),
+        "t5": (_pose(25.0, 0.09, 0.05),),
+    }
+    selected_first, first_reason = tracker.select(
+        first,
+        stamp_ns=1_000_000_000,
+    )
+    assert first_reason == ""
+    assert selected_first is not None
+
+    # The per-tag minimum-error solutions move only a few degrees and can
+    # therefore fool independent trackers. Together they change T4->T5 by
+    # 8 deg; the slightly higher-error pair preserves the physical chain.
+    continuous_t4 = _pose(16.0, 0.061, 0.20)
+    continuous_t5 = _pose(26.0, 0.091, 0.20)
+    independent_best_t4 = _pose(19.0, 0.061, 0.05)
+    independent_best_t5 = _pose(21.0, 0.091, 0.05)
+    second = {
+        "t0": (_pose(0.2, 0.00, 0.05),),
+        "t3": (_pose(5.2, 0.03, 0.05),),
+        "t4": (independent_best_t4, continuous_t4),
+        "t5": (independent_best_t5, continuous_t5),
+    }
+    selected, reason = tracker.select(
+        second,
+        stamp_ns=1_033_000_000,
+    )
+
+    assert reason == ""
+    assert selected is not None
+    assert selected["t4"] == continuous_t4
+    assert selected["t5"] == continuous_t5
+    assert tracker.branch_correction_counts == {"t4": 1, "t5": 1}
+
+
+def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("t4", "t5"),
+        adjacent_pairs=(("t4", "t5"),),
+        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,
+    )
+    true_t4 = _pose(30.0, 0.06, 0.05)
+    true_t5 = _pose(65.0, 0.09, 0.05)
+    selected, _ = tracker.select(
+        {"t4": (true_t4,), "t5": (true_t5,)},
+        stamp_ns=1_000_000_000,
+    )
+    assert selected is not None
+
+    return_t4 = _pose(29.5, 0.06, 0.20)
+    return_t5 = _pose(64.5, 0.09, 0.20)
+    mirror_t4 = _pose(33.0, 0.06, 0.04)
+    mirror_t5 = _pose(57.0, 0.09, 0.04)
+    selected, reason = tracker.select(
+        {
+            "t4": (mirror_t4, return_t4),
+            "t5": (mirror_t5, return_t5),
+        },
+        stamp_ns=1_033_000_000,
+    )
+
+    assert reason == ""
+    assert selected == {"t4": return_t4, "t5": return_t5}
+
+
+def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None:
+    roles = ("t3", "t4", "t5")
+    mount_rotations = {
+        "t3": Rotation.identity(),
+        "t4": Rotation.from_euler("z", 20.0, degrees=True),
+        "t5": Rotation.from_euler("z", -15.0, degrees=True),
+    }
+    mount_positions = {
+        "t3": np.asarray([0.0, 0.0, 0.0]),
+        "t4": np.asarray([0.025, 0.0, 0.0]),
+        "t5": np.asarray([0.05, 0.0, 0.0]),
+    }
+    false_factors = {"t3": 0.5, "t4": -0.5, "t5": 1.0}
+    frames = []
+    true_frames = []
+    for angle_deg in np.linspace(0.0, 45.0, 30):
+        group_rotation = Rotation.from_euler(
+            "y", angle_deg, degrees=True
+        )
+        origin = np.asarray([0.0, 0.0, 0.3])
+        candidates = {}
+        truths = {}
+        for role in roles:
+            true_rotation = group_rotation * mount_rotations[role]
+            true_translation = origin + group_rotation.apply(
+                mount_positions[role]
+            )
+            true_pose = SquareTagPose(
+                quaternion_xyzw=tuple(true_rotation.as_quat()),
+                translation_xyz_m=tuple(true_translation),
+                reprojection_error_px=0.10,
+            )
+            false_rotation = true_rotation * Rotation.from_euler(
+                "x",
+                false_factors[role] * angle_deg,
+                degrees=True,
+            )
+            false_pose = SquareTagPose(
+                quaternion_xyzw=tuple(false_rotation.as_quat()),
+                translation_xyz_m=tuple(
+                    true_translation
+                    + np.asarray(
+                        [
+                            0.0,
+                            false_factors[role] * angle_deg / 10000.0,
+                            0.0,
+                        ]
+                    )
+                ),
+                reprojection_error_px=0.05,
+            )
+            candidates[role] = (false_pose, true_pose)
+            truths[role] = true_pose
+        frames.append(candidates)
+        true_frames.append(truths)
+
+    selected, quality = select_rigid_group_trajectory(
+        frames,
+        roles=roles,
+        fixed_pairs=(("t3", "t4"), ("t4", "t5")),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+    )
+
+    for role in roles:
+        assert (
+            rotation_distance_rad(
+                selected[-1][role].quaternion_xyzw,
+                true_frames[-1][role].quaternion_xyzw,
+            )
+            < 1.0e-8
+        )
+    assert quality["maximum_pair_rotation_drift_rad"] < np.deg2rad(5.0)
+    assert quality["p95_pair_rotation_drift_rad"] < np.deg2rad(5.0)
+
+
+def test_trajectory_quality_uses_robust_rigid_reference() -> None:
+    frames = []
+    for index in range(30):
+        child_rotation = Rotation.identity()
+        if index == 0:
+            child_rotation = Rotation.from_euler(
+                "x", 10.0, degrees=True
+            )
+        frames.append(
+            {
+                "parent": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(
+                            Rotation.identity().as_quat()
+                        ),
+                        translation_xyz_m=(0.0, 0.0, 0.3),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+                "child": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(child_rotation.as_quat()),
+                        translation_xyz_m=(0.03, 0.0, 0.3),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+            }
+        )
+
+    _, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+    )
+
+    assert quality["maximum_pair_rotation_drift_rad"] == pytest.approx(
+        np.deg2rad(10.0)
+    )
+    assert quality["p95_pair_rotation_drift_rad"] == pytest.approx(0.0)
+    assert quality["median_pair_rotation_drift_rad"] == pytest.approx(0.0)
+
+
+def test_distance_geometry_ignores_planar_orientation_drift() -> None:
+    frames = []
+    for angle_deg in np.linspace(0.0, 45.0, 30):
+        group = Rotation.from_euler("y", angle_deg, degrees=True)
+        origin = np.asarray([0.0, 0.0, 0.3])
+        parent_position = origin
+        child_position = origin + group.apply([0.04, 0.0, 0.0])
+        # The centres form a perfect rigid pair, while the planar-PnP parent
+        # orientation contains a pose-dependent error.
+        parent_rotation = group * Rotation.from_euler(
+            "z", 0.25 * angle_deg, degrees=True
+        )
+        frames.append(
+            {
+                "parent": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(parent_rotation.as_quat()),
+                        translation_xyz_m=tuple(parent_position),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+                "child": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(group.as_quat()),
+                        translation_xyz_m=tuple(child_position),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+            }
+        )
+
+    _, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+        pair_geometry="distance",
+    )
+
+    assert quality["pair_geometry"] == "distance"
+    assert quality["maximum_pair_distance_drift_m"] < 1.0e-10
+    assert quality["p95_pair_distance_drift_m"] < 1.0e-10
+    assert quality["p95_pair_translation_drift_m"] > 0.001
+
+
+def test_distance_geometry_rejects_pose_branch_with_changing_length() -> None:
+    frames = []
+    true_children = []
+    for index in range(30):
+        parent = SquareTagPose(
+            quaternion_xyzw=tuple(Rotation.identity().as_quat()),
+            translation_xyz_m=(0.0, 0.0, 0.3),
+            reprojection_error_px=0.1,
+        )
+        true_child = SquareTagPose(
+            quaternion_xyzw=tuple(Rotation.identity().as_quat()),
+            translation_xyz_m=(0.04, 0.0, 0.3),
+            reprojection_error_px=0.1,
+        )
+        false_child = SquareTagPose(
+            quaternion_xyzw=tuple(
+                Rotation.from_euler("x", 10.0, degrees=True).as_quat()
+            ),
+            translation_xyz_m=(
+                0.04,
+                0.020 * np.sin(np.pi * index / 29.0),
+                0.3,
+            ),
+            reprojection_error_px=0.05,
+        )
+        frames.append(
+            {
+                "parent": (parent,),
+                "child": (false_child, true_child),
+            }
+        )
+        true_children.append(true_child)
+
+    selected, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.002,
+        pair_geometry="distance",
+    )
+
+    assert selected[len(selected) // 2]["child"] == (
+        true_children[len(true_children) // 2]
+    )
+    assert quality["p95_pair_distance_drift_m"] < 1.0e-6

src/g20_thumb_apriltag_calibration/test/test_storage.py

@@ -0,0 +1,35 @@
+from g20_thumb_apriltag_calibration.storage import (
+    append_jsonl,
+    atomic_write_json,
+    completed_scan_keys,
+    load_json,
+    load_jsonl,
+)
+
+
+def test_jsonl_checkpoint_and_resume_keys(tmp_path) -> None:
+    raw_path = tmp_path / "raw_samples.jsonl"
+    record = {
+        "kind": "sample",
+        "phase": "root",
+        "cycle": 0,
+        "direction": "decreasing",
+        "command_u8": 255,
+    }
+    append_jsonl(raw_path, record)
+    append_jsonl(raw_path, {"kind": "validation", "command_u8": 10})
+    loaded = load_jsonl(raw_path)
+    assert loaded[0] == record
+    assert completed_scan_keys(loaded) == {("root", 0, "decreasing", 255)}
+
+    checkpoint_path = tmp_path / "checkpoint.json"
+    atomic_write_json(checkpoint_path, {"state": "PAUSED", "records": 1})
+    assert load_json(checkpoint_path) == {"state": "PAUSED", "records": 1}
+
+
+def test_resume_ignores_only_a_truncated_final_jsonl_record(tmp_path) -> None:
+    raw_path = tmp_path / "raw_samples.jsonl"
+    append_jsonl(raw_path, {"kind": "sample", "phase": "root"})
+    with raw_path.open("a", encoding="utf-8") as stream:
+        stream.write('{"kind":"sample"\n\n')
+    assert load_jsonl(raw_path) == [{"kind": "sample", "phase": "root"}]

src/g20_thumb_apriltag_calibration/test/test_trajectory.py

@@ -0,0 +1,295 @@
+from __future__ import annotations
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.core import (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+    PAIR_IP,
+    PAIR_MCP,
+    PAIR_ROOT,
+    PHASE_ROOT,
+    PHASE_TIP,
+    create_final_payload,
+)
+from g20_thumb_apriltag_calibration.trajectory import (
+    _regularize_coupled_zero_tail,
+    fit_center_trajectory_curves,
+    maximum_center_non_target_drift_rad,
+    measure_center_trajectory_angles,
+)
+
+
+def _quat(angle: float) -> list[float]:
+    return [
+        float(value)
+        for value in Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
+    ]
+
+
+def _records(
+    *,
+    camera_rotation: Rotation = Rotation.identity(),
+    camera_translation: np.ndarray = np.zeros(3),
+    tag_shift: float = 0.0,
+) -> list[dict]:
+    commands = list(range(0, 256, 8))
+    if commands[-1] != 255:
+        commands.append(255)
+    directions = (
+        (DIRECTION_DECREASING, list(reversed(commands))),
+        (DIRECTION_INCREASING, commands),
+    )
+    t0 = np.asarray([0.0, 0.0, 0.55])
+    root_centre = np.asarray([0.025, -0.010, 0.55])
+    root_points = {
+        "t3": np.asarray([0.060 + tag_shift, -0.005, 0.55]),
+        "t4": np.asarray([0.090, 0.002 + tag_shift, 0.55]),
+        "t5": np.asarray([0.120, 0.009, 0.55 + tag_shift]),
+    }
+    mcp_centre = np.asarray([0.055, -0.004, 0.0])
+    t3_tip = np.asarray([0.0, 0.0, 0.55])
+    t4_reference = t3_tip + np.asarray([0.080, 0.006 + tag_shift, 0.0])
+    ip_centre = t3_tip + np.asarray([0.095, 0.006, 0.0])
+    t5_reference = t3_tip + np.asarray([0.125, 0.008 + tag_shift, 0.0])
+
+    def camera(point: np.ndarray) -> np.ndarray:
+        return camera_rotation.apply(point) + camera_translation
+
+    records: list[dict] = []
+    for phase in (PHASE_ROOT, PHASE_TIP):
+        for direction, ordered_commands in directions:
+            for command in ordered_commands:
+                progress = (255.0 - command) / 255.0
+                root_angle = 0.80 * progress if phase == PHASE_ROOT else 0.0
+                mcp_angle = 1.15 * progress if phase == PHASE_TIP else 0.0
+                ip_angle = 1.02 * mcp_angle if phase == PHASE_TIP else 0.0
+                if phase == PHASE_ROOT:
+                    root_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, root_angle]
+                    )
+                    positions = {
+                        "t0": t0,
+                        **{
+                            role: root_centre
+                            + root_rotation.apply(point - root_centre)
+                            for role, point in root_points.items()
+                        },
+                    }
+                else:
+                    mcp_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, mcp_angle]
+                    )
+                    ip_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, ip_angle]
+                    )
+                    # The MCP centre below is expressed relative to T3.
+                    mcp_world = t3_tip + mcp_centre
+                    t4 = mcp_world + mcp_rotation.apply(
+                        t4_reference - mcp_world
+                    )
+                    ip_at_zero = ip_centre
+                    t5_inside_parent = ip_at_zero + ip_rotation.apply(
+                        t5_reference - ip_at_zero
+                    )
+                    t5 = mcp_world + mcp_rotation.apply(
+                        t5_inside_parent - mcp_world
+                    )
+                    positions = {
+                        "t0": t0,
+                        "t3": t3_tip,
+                        "t4": t4,
+                        "t5": t5,
+                    }
+                records.append(
+                    {
+                        "kind": "sample",
+                        "phase": phase,
+                        "cycle": 0,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_quaternion_xyzw": {
+                            PAIR_ROOT: _quat(root_angle),
+                            PAIR_MCP: _quat(mcp_angle),
+                            PAIR_IP: _quat(ip_angle),
+                        },
+                        "tag_translation_xyz_m": {
+                            role: [
+                                float(value) for value in camera(point)
+                            ]
+                            for role, point in positions.items()
+                        },
+                    }
+                )
+    return records
+
+
+def test_centre_trajectory_recovers_three_joint_angles_and_zero() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(
+        records,
+        maximum_plane_rms_m=0.001,
+        maximum_radial_rms_m=0.001,
+        maximum_anchor_drift_m=0.001,
+    )
+    assert fit.measurement_mode == "trajectory_center_3d"
+    assert fit.joints["thumb_cmc_pitch"]["angle_rad"][0] == pytest.approx(
+        0.80, abs=2.0e-3
+    )
+    assert fit.joints["thumb_mcp"]["angle_rad"][0] == pytest.approx(
+        1.15, abs=2.0e-3
+    )
+    assert fit.joints["thumb_ip"]["angle_rad"][0] == pytest.approx(
+        1.173, abs=3.0e-3
+    )
+    for joint in fit.joints.values():
+        assert joint["angle_rad"][255] == 0.0
+    payload = create_final_payload(
+        serial_number="G20_LEFT_TRAJECTORY_TEST",
+        fit=fit,
+        validation_errors_rad=[0.01, -0.01],
+        passed=True,
+    )
+    assert payload["zero_command_u8"] == 255
+    for joint in payload["joints"].values():
+        assert len(joint["angle_rad"]) == 256
+        assert joint["angle_rad"][255] == 0.0
+
+
+def test_centre_trajectory_is_invariant_to_camera_and_tag_offset() -> None:
+    reference = fit_center_trajectory_curves(_records())
+    changed = fit_center_trajectory_curves(
+        _records(
+            camera_rotation=Rotation.from_euler(
+                "xyz", [0.35, -0.25, 0.20]
+            ),
+            camera_translation=np.asarray([0.12, -0.04, 0.08]),
+            tag_shift=0.004,
+        )
+    )
+    for joint_name in ("thumb_cmc_pitch", "thumb_mcp", "thumb_ip"):
+        assert changed.joints[joint_name]["angle_rad"] == pytest.approx(
+            reference.joints[joint_name]["angle_rad"],
+            abs=6.0e-3,
+        )
+
+
+def test_passive_ip_uses_mimic_constraint_despite_distal_pnp_bias() -> None:
+    reference = fit_center_trajectory_curves(_records())
+    biased_records = _records(
+        camera_rotation=Rotation.from_euler(
+            "xyz", [-0.28, 0.31, -0.16]
+        ),
+        camera_translation=np.asarray([-0.08, 0.03, 0.11]),
+    )
+    for record in biased_records:
+        if record["phase"] != PHASE_TIP:
+            continue
+        progress = (255.0 - float(record["command_u8"])) / 255.0
+        bias = np.asarray(
+            [
+                0.0012 * np.sin(1.7 * progress),
+                0.0008 * progress * progress,
+                -0.0006 * np.sin(2.3 * progress),
+            ]
+        )
+        record["tag_translation_xyz_m"]["t5"] = [
+            float(value)
+            for value in (
+                np.asarray(
+                    record["tag_translation_xyz_m"]["t5"], dtype=float
+                )
+                + bias
+            )
+        ]
+
+    biased = fit_center_trajectory_curves(biased_records)
+    for fit in (reference, biased):
+        mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"])
+        ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"])
+        assert ip == pytest.approx(1.02 * mcp, abs=1.1e-8)
+        assert fit.ip_coupling["multiplier"] == pytest.approx(1.02)
+        assert fit.ip_coupling["offset_rad"] == 0.0
+        assert fit.ip_coupling["constrained_r_squared"] == 1.0
+        assert fit.ip_coupling["r_squared"] > 0.98
+
+    assert biased.joints["thumb_ip"]["angle_rad"] == pytest.approx(
+        reference.joints["thumb_ip"]["angle_rad"],
+        abs=6.0e-3,
+    )
+    assert (
+        biased.trajectory_quality["tip"][
+            "ip_observed_vs_constrained_max_rad"
+        ]
+        > 0.0
+    )
+
+
+def test_passive_ip_multiplier_is_configurable() -> None:
+    fit = fit_center_trajectory_curves(
+        _records(),
+        passive_ip_multiplier=0.97,
+    )
+    mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"])
+    ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"])
+    assert ip == pytest.approx(0.97 * mcp, abs=1.1e-8)
+
+
+def test_static_measurement_uses_fitted_serial_tip_model() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(records)
+    command = 128
+    record = next(
+        item
+        for item in records
+        if item["phase"] == PHASE_TIP
+        and item["direction"] == DIRECTION_DECREASING
+        and item["command_u8"] == command
+    )
+    measured = measure_center_trajectory_angles(
+        fit.trajectory_models,
+        record["tag_translation_xyz_m"],
+    )
+    progress = (255.0 - command) / 255.0
+    assert measured["thumb_mcp"] == pytest.approx(
+        1.15 * progress, abs=2.0e-3
+    )
+    assert measured["thumb_ip"] == pytest.approx(
+        1.02 * 1.15 * progress, abs=3.0e-3
+    )
+
+
+def test_non_target_drift_is_measured_without_tag_orientations() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(records)
+    assert maximum_center_non_target_drift_rad(
+        records, fit.trajectory_models
+    ) == pytest.approx(0.0, abs=3.0e-3)
+
+
+def test_short_ip_zero_tail_uses_coupled_mcp_shape() -> None:
+    mcp = np.linspace(1.0, 0.0, 256)
+    ip = 0.6 * mcp
+    ip[248:] = 0.0
+
+    regularized = _regularize_coupled_zero_tail(ip, mcp)
+
+    assert regularized[:248] == pytest.approx(ip[:248])
+    assert np.all(regularized[248:255] > 0.0)
+    assert np.all(np.diff(regularized) <= 1.0e-12)
+    assert regularized[255] == 0.0
+    assert regularized[248:255] == pytest.approx(
+        0.6 * mcp[248:255]
+    )
+
+
+def test_long_or_unresolved_ip_zero_tail_is_not_invented() -> None:
+    mcp = np.linspace(1.0, 0.0, 256)
+    ip = 0.6 * mcp
+    ip[220:] = 0.0
+
+    regularized = _regularize_coupled_zero_tail(ip, mcp)
+
+    assert regularized == pytest.approx(ip)

src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py

@@ -0,0 +1,340 @@
+from __future__ import annotations
+
+import math
+
+import cv2
+from g20_thumb_apriltag_calibration.zero_calibration import (
+    build_trajectory_zero_angle_payload,
+    build_trajectory_zero_travel_payload,
+    circular_median_rad,
+    detect_reference_alignment_line,
+    fit_image_circle_trajectory,
+    measure_zero_from_circle,
+    signed_angle_difference_rad,
+    summarize_zero_frames,
+    validate_zero_angle_payload,
+    validate_zero_travel_payload,
+)
+import numpy as np
+import pytest
+
+
+def _detect_reference_line(image: np.ndarray, reference_y: float) -> dict | None:
+    return detect_reference_alignment_line(
+        image,
+        reference_y_px=reference_y,
+        roi_y_min_ratio=0.55,
+        roi_y_max_ratio=0.98,
+        minimum_length_ratio=0.30,
+        maximum_candidate_angle_rad=math.radians(15.0),
+    )
+
+
+def _frame(
+    table_angle_rad: float,
+    *,
+    state_u8: float = 255.0,
+    t0_xy: tuple[float, float] = (500.0, 300.0),
+    circle_xy: tuple[float, float] = (-120.0, 80.0),
+    radius_px: float = 90.0,
+) -> dict[str, float]:
+    # table_angle_rad uses a y-up convention while image y grows downwards.
+    relative = np.asarray(circle_xy) + radius_px * np.asarray(
+        [math.cos(table_angle_rad), -math.sin(table_angle_rad)]
+    )
+    t0 = np.asarray(t0_xy)
+    t3 = t0 + relative
+    return {
+        "state_u8": state_u8,
+        "t0_x_px": float(t0[0]),
+        "t0_y_px": float(t0[1]),
+        "t3_x_px": float(t3[0]),
+        "t3_y_px": float(t3[1]),
+    }
+
+
+def _trajectory(noise_px: float = 0.15) -> list[dict[str, float]]:
+    rng = np.random.default_rng(7)
+    observations: list[dict[str, float]] = []
+    for states in (
+        np.linspace(255.0, 64.0, 70),
+        np.linspace(64.0, 255.0, 70),
+    ):
+        for state in states:
+            fraction = (255.0 - state) / (255.0 - 64.0)
+            angle = math.radians(10.0 + 65.0 * fraction)
+            shift = rng.normal(0.0, 0.35, size=2)
+            frame = _frame(
+                angle,
+                state_u8=float(state),
+                t0_xy=(500.0 + shift[0], 300.0 + shift[1]),
+            )
+            frame["t3_x_px"] += float(rng.normal(0.0, noise_px))
+            frame["t3_y_px"] += float(rng.normal(0.0, noise_px))
+            observations.append(frame)
+    return observations
+
+
+def _fit(observations: list[dict[str, float]] | None = None) -> dict:
+    return fit_image_circle_trajectory(
+        _trajectory() if observations is None else observations,
+        bin_size_u8=8.0,
+        minimum_frames=45,
+        minimum_bins=18,
+        minimum_state_span_u8=160.0,
+        minimum_radius_px=20.0,
+        minimum_arc_rad=math.radians(20.0),
+        maximum_radial_rms_px=2.0,
+        maximum_p95_radial_error_px=3.5,
+    )
+
+
+def _zero_summary(angle_rad: float) -> dict[str, float]:
+    frames = [
+        _frame(
+            angle_rad + math.radians(index - 14.5) * 1.0e-4,
+        )
+        for index in range(30)
+    ]
+    return summarize_zero_frames(frames)
+
+
+def _measurement(
+    table_rad: float,
+    *,
+    radial_error_px: float = 0.2,
+) -> dict[str, float]:
+    return {
+        "table_rad": table_rad,
+        "zero_radial_error_px": radial_error_px,
+    }
+
+
+def _round(
+    table_rad: float,
+    *,
+    return_delta: float = 0.002,
+) -> dict[str, dict[str, float]]:
+    return {
+        "zero_before": _measurement(table_rad),
+        "zero_after": _measurement(
+            table_rad + return_delta,
+        ),
+    }
+
+
+def _travel_round(
+    zero_table_rad: float,
+    travel_rad: float,
+    *,
+    return_delta: float = 0.002,
+) -> dict[str, dict[str, float]]:
+    round_value = _round(
+        zero_table_rad,
+        return_delta=return_delta,
+    )
+    round_value["travel_endpoint"] = _measurement(
+        zero_table_rad + travel_rad,
+    )
+    return round_value
+
+
+def test_circular_statistics_cross_pi_without_jumping() -> None:
+    values = [math.radians(179.0), math.radians(-179.0), math.pi]
+    result = circular_median_rad(values)
+    assert abs(signed_angle_difference_rad(result, math.pi)) < math.radians(1.1)
+
+
+def test_detect_reference_line_reports_signed_angle_and_offset() -> None:
+    height, width = 720, 1280
+    reference_y = 0.90 * (height - 1)
+    expected_angle = math.radians(2.0)
+    image = np.zeros((height, width, 3), dtype=np.uint8)
+    half_span = 560.0
+    vertical_change = math.tan(expected_angle) * half_span
+    cv2.line(
+        image,
+        (80, int(round(reference_y + vertical_change))),
+        (1200, int(round(reference_y - vertical_change))),
+        (255, 255, 255),
+        5,
+    )
+
+    result = _detect_reference_line(image, reference_y)
+
+    assert result is not None
+    assert result["angle_rad"] == pytest.approx(expected_angle, abs=0.004)
+    assert abs(result["vertical_offset_px"]) <= 5.0
+
+
+def test_detect_reference_line_rejects_non_horizontal_scene() -> None:
+    image = np.zeros((720, 1280, 3), dtype=np.uint8)
+    cv2.line(image, (640, 420), (640, 700), (255, 255, 255), 5)
+
+    assert _detect_reference_line(image, 0.90 * 719.0) is None
+
+
+def test_circle_fit_recovers_center_radius_and_rejects_anchor_translation() -> None:
+    fit = _fit()
+    assert fit["centre_relative_xy_px"] == pytest.approx(
+        [-120.0, 80.0], abs=0.8
+    )
+    assert fit["radius_px"] == pytest.approx(90.0, abs=0.8)
+    assert fit["arc_rad"] >= math.radians(60.0)
+    assert fit["radial_rms_px"] < 0.5
+    assert fit["passed"] is True
+
+
+def test_circle_fit_is_robust_to_sparse_bad_tag_centres() -> None:
+    observations = _trajectory()
+    for index in (9, 31, 57, 92, 121):
+        observations[index]["t3_x_px"] += 18.0
+        observations[index]["t3_y_px"] -= 15.0
+    fit = _fit(observations)
+    assert fit["centre_relative_xy_px"] == pytest.approx(
+        [-120.0, 80.0], abs=1.5
+    )
+    assert fit["radius_px"] == pytest.approx(90.0, abs=1.5)
+
+
+def test_zero_uses_fixed_inward_radius_and_ignores_t3_rotation() -> None:
+    circle = _fit()
+    trajectory_angle = math.radians(10.0)
+    expected_table = math.radians(-170.0)
+    summary = _zero_summary(trajectory_angle)
+    summary_with_t3_forward = {**summary, "t3_rad": math.radians(31.0)}
+    summary_with_t3_flipped = {
+        **summary,
+        "t3_rad": math.radians(-149.0),
+    }
+    result = measure_zero_from_circle(circle, summary)
+    forward = measure_zero_from_circle(circle, summary_with_t3_forward)
+    flipped = measure_zero_from_circle(circle, summary_with_t3_flipped)
+    assert result["table_rad"] == pytest.approx(expected_table, abs=0.01)
+    assert forward == result
+    assert flipped == result
+
+
+def test_circle_fit_rejects_short_state_span() -> None:
+    observations = [
+        _frame(math.radians(10.0 + index * 0.1), state_u8=255.0 - index)
+        for index in range(50)
+    ]
+    with pytest.raises(ValueError, match="trajectory_state_span_too_small"):
+        _fit(observations)
+
+
+def test_payload_remains_small_and_contains_circle_derived_angles() -> None:
+    rounds = [
+        _round(-0.30),
+        _round(-0.299),
+        _round(-0.302),
+    ]
+    payload, report = build_trajectory_zero_angle_payload(
+        serial_number="G20_LEFT_TEST",
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=0.99,
+        minimum_detection_rate=0.95,
+    )
+    validate_zero_angle_payload(payload)
+    assert payload["schema_version"] == 2
+    assert set(payload["zero_angles"]) == {"table_projected_zero_rad"}
+    assert payload["zero_angles"]["table_projected_zero_rad"] == pytest.approx(
+        -0.299
+    )
+    assert payload["quality"]["passed"] is True
+    assert report["measurement_method"] == (
+        "t3_center_to_circle_centre_image_trajectory"
+    )
+
+
+@pytest.mark.parametrize(("detection_rate", "radial_error"), [(0.8, 0.2), (0.99, 8.0)])
+def test_quality_fails_for_detection_or_zero_circle_error(
+    detection_rate: float, radial_error: float
+) -> None:
+    rounds = [
+        {
+            "zero_before": _measurement(
+                -0.30, radial_error_px=radial_error
+            ),
+            "zero_after": _measurement(
+                -0.299, radial_error_px=radial_error
+            ),
+        }
+        for _ in range(3)
+    ]
+    payload, _ = build_trajectory_zero_angle_payload(
+        serial_number="G20_LEFT_TEST",
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=detection_rate,
+        minimum_detection_rate=0.95,
+    )
+    assert payload["quality"]["passed"] is False
+
+
+def test_roll_payload_contains_zero_and_measured_travel() -> None:
+    rounds = [
+        _travel_round(-0.30, 1.201),
+        _travel_round(-0.299, 1.199),
+        _travel_round(-0.302, 1.200),
+    ]
+    payload, report = build_trajectory_zero_travel_payload(
+        serial_number="G20_LEFT_TEST",
+        joint_name="thumb_cmc_roll",
+        zero_command_u8=255,
+        travel_endpoint_command_u8=0,
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_travel_difference_rad=math.radians(1.0),
+        minimum_travel_rad=math.radians(20.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=0.99,
+        minimum_detection_rate=0.95,
+    )
+
+    validate_zero_travel_payload(payload)
+    assert payload["joint"] == "thumb_cmc_roll"
+    assert payload["schema_version"] == 2
+    assert payload["zero_command_u8"] == 255
+    assert payload["travel_endpoint_command_u8"] == 0
+    assert set(payload["zero_angles"]) == {"table_projected_zero_rad"}
+    assert payload["travel"]["signed_rad"] == pytest.approx(1.2, abs=0.003)
+    assert payload["travel"]["range_rad"] == pytest.approx(1.2, abs=0.003)
+    assert payload["quality"]["passed"] is True
+    assert report["signed_travel_rounds_rad"] == pytest.approx(
+        [1.2, 1.198, 1.199], abs=0.003
+    )
+
+
+def test_roll_quality_rejects_inconsistent_or_short_travel() -> None:
+    rounds = [
+        _travel_round(-0.30, travel)
+        for travel in (0.10, 0.12, 0.14)
+    ]
+    payload, _ = build_trajectory_zero_travel_payload(
+        serial_number="G20_LEFT_TEST",
+        joint_name="thumb_cmc_roll",
+        zero_command_u8=255,
+        travel_endpoint_command_u8=0,
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_travel_difference_rad=math.radians(1.0),
+        minimum_travel_rad=math.radians(20.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=0.99,
+        minimum_detection_rate=0.95,
+    )
+
+    assert payload["quality"]["passed"] is False