标定

.gitignore

@@ -60,6 +60,7 @@ Thumbs.db
 # Reproducible seed profiles remain under
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
+/calibration_output/
 *.wear_check.json
 *.checkpoint.json
 *.verification.json

src/g20_thumb_apriltag_calibration/README.md

@@ -0,0 +1,196 @@
+# G20 左手拇指正面 AprilTag 标定
+
+该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
+只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
+终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
+程序把每帧 AprilTag 角度与同一时刻的实际电机位置插值配对并按整数位置分箱。
+完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点
+平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。
+
+当前正面单机位使用 `image_plane_2d` 模式：从四个有序角点计算 Tag 在校正图像平面内的
+方向，再求 T0→T3、T3→T4、T4→T5 的相对转角。该模式只用于正面屈伸角，不估计侧摆、
+横摆或出平面旋转；AprilTag 的 PnP/TF 仍保留作诊断和数据归档。
+
+## 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 \
+  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
+```
+
+若以后需要以帧率优先，可传入 `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。连续扫描要求
+图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、
+至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、
+90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到
+该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列，
+不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。
+
+单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用
+实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待
+电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路，
+不要降低到固件低速区。必须临时回退时可启动
+`continuous_motion_mode:=paced`，该模式按步长 8 到位即发下一段。
+
+连续扫描中的主要状态字段：
+
+- `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`：当前是根部或尖部、下降或上升方向。
+
+## 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`：连续帧按实际整数电机位置分箱后的鲁棒统计及复测点；每完成一个
+  扫描方向后落盘。
+- `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` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留
+在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。
+
+如果相机或 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 检测。校正和 AprilTag 组件运行
+在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
+可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
+不订阅整幅图像，只订阅检测结果和 TF。
+若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
+不影响 AprilTag 的 ROI 输入。
+
+静态预检会把偏离鲁棒姿态超过 5° 的平面 PnP 瞬时翻解视为异常帧，但要求姿态内点率
+至少 95%；内点自身仍必须满足 0.5° RMS，避免用异常过滤掩盖真实抖动。
+
+启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
+订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
+若用于正式可追溯验收，再启用并检查磁盘空间。

src/g20_thumb_apriltag_calibration/config/calibration.yaml

@@ -0,0 +1,69 @@
+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
+    angle_estimation_mode: image_plane_2d
+    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
+    maximum_static_std_deg: 0.5
+    pose_outlier_threshold_deg: 5.0
+    minimum_pose_inlier_rate: 0.95
+
+    # Static captures are now used only for sweep preparation and validation.
+    stable_frames: 5
+    capture_frames: 8
+    minimum_settle_seconds: 0.4
+    maximum_stable_spread_deg: 0.3
+    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/front_tags.yaml

@@ -0,0 +1,26 @@
+/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    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,436 @@
+"""Hardware-independent point acquisition state."""
+
+from __future__ import annotations
+
+from bisect import bisect_left
+from collections import deque
+from dataclasses import dataclass
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .core import PAIR_NAMES, robust_rotation_summary
+
+
+@dataclass(frozen=True)
+class TagQuality:
+    hamming: int
+    decision_margin: float
+    edge_pixels: float
+
+
+def tag_quality_is_valid(
+    quality: TagQuality,
+    *,
+    maximum_hamming: int,
+    minimum_decision_margin: float,
+    minimum_edge_pixels: float,
+) -> bool:
+    return (
+        quality.hamming <= maximum_hamming
+        and quality.decision_margin >= minimum_decision_margin
+        and quality.edge_pixels >= minimum_edge_pixels
+    )
+
+
+@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
+
+
+@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),
+        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.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 = ""
+
+    def start(self, now: float) -> None:
+        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 = ""
+
+    @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
+        for pair in PAIR_NAMES:
+            quaternions = [
+                observation.relative_quaternion_xyzw[pair]
+                for observation in self._stable
+            ]
+            _, spread = robust_rotation_summary(quaternions)
+            if spread > self.maximum_stable_spread_rad:
+                self.reason = f"{pair}_not_stable"
+                return False
+        return True
+
+    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 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
+        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 == "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)),
+        }
+
+    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
+    ]
+
+    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,
+        "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,633 @@
+"""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
+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, 15):
+        raise ValueError("front thumb calibration only permits motor 0 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
+
+    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/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/launch/front_thumb_calibration.launch.py

@@ -0,0 +1,356 @@
+"""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,
+                ),
+                "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("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,33 @@
+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",
+        ],
+    },
+)

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

@@ -0,0 +1,199 @@
+from __future__ import annotations
+
+from dataclasses import replace
+
+import numpy as np
+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 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_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")
+
+
+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_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,56 @@
+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["detector"]["decimate"] == 1.5
+    assert detector["detector"]["refine"] is True
+    assert detector["detector"]["debug"] is False
+
+
+def test_trial_uses_image_plane_angles_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"] == "image_plane_2d"
+    assert parameters["minimum_edge_pixels"] == 30.0
+    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["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"

src/g20_thumb_apriltag_calibration/test/test_core.py

@@ -0,0 +1,240 @@
+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:])
+    with pytest.raises(ValueError):
+        build_command(5, 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_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"}]