From 44975620a71386a31af0e6660907331abeba8a28 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Wed, 29 Jul 2026 11:49:16 +0800 Subject: [PATCH] =?UTF-8?q?=E6=A0=87=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 1 + src/g20_thumb_apriltag_calibration/README.md | 196 ++ .../config/calibration.yaml | 69 + .../config/front_tags.yaml | 26 + .../config/realsense_color_qos.yaml | 5 + .../__init__.py | 5 + .../acquisition.py | 436 ++++ .../g20_thumb_apriltag_calibration/core.py | 633 ++++++ .../g20_thumb_apriltag_calibration/node.py | 1840 +++++++++++++++++ .../g20_thumb_apriltag_calibration/storage.py | 87 + .../launch/front_thumb_calibration.launch.py | 356 ++++ .../package.xml | 35 + .../pyproject.toml | 3 + .../resource/g20_thumb_apriltag_calibration | 1 + src/g20_thumb_apriltag_calibration/setup.cfg | 4 + src/g20_thumb_apriltag_calibration/setup.py | 33 + .../test/test_acquisition.py | 199 ++ .../test/test_config.py | 56 + .../test/test_core.py | 240 +++ .../test/test_storage.py | 35 + 20 files changed, 4260 insertions(+) create mode 100644 src/g20_thumb_apriltag_calibration/README.md create mode 100644 src/g20_thumb_apriltag_calibration/config/calibration.yaml create mode 100644 src/g20_thumb_apriltag_calibration/config/front_tags.yaml create mode 100644 src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py create mode 100644 src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py create mode 100644 src/g20_thumb_apriltag_calibration/package.xml create mode 100644 src/g20_thumb_apriltag_calibration/pyproject.toml create mode 100644 src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration create mode 100644 src/g20_thumb_apriltag_calibration/setup.cfg create mode 100644 src/g20_thumb_apriltag_calibration/setup.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_acquisition.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_config.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_core.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_storage.py diff --git a/.gitignore b/.gitignore index c100af8..eae41ab 100644 --- a/.gitignore +++ b/.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 diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md new file mode 100644 index 0000000..392a5f1 --- /dev/null +++ b/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 分卷。快速标定通常不需要录制； +若用于正式可追溯验收，再启用并检查磁盘空间。 diff --git a/src/g20_thumb_apriltag_calibration/config/calibration.yaml b/src/g20_thumb_apriltag_calibration/config/calibration.yaml new file mode 100644 index 0000000..68d095e --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/config/front_tags.yaml b/src/g20_thumb_apriltag_calibration/config/front_tags.yaml new file mode 100644 index 0000000..0c385eb --- /dev/null +++ b/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] diff --git a/src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml b/src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml new file mode 100644 index 0000000..1261ee8 --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py new file mode 100644 index 0000000..b7d1225 --- /dev/null +++ b/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"] diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py new file mode 100644 index 0000000..f423b04 --- /dev/null +++ b/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 + ), + } diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py new file mode 100644 index 0000000..75d7ce4 --- /dev/null +++ b/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") diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py new file mode 100644 index 0000000..0b4fde5 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py @@ -0,0 +1,1840 @@ +"""ROS 2 hardware calibration node for a front-facing RealSense camera.""" + +from __future__ import annotations + +from collections import deque +from datetime import datetime, timezone +import hashlib +import json +import math +from pathlib import Path +import random +import re +import time +from typing import Any + +import cv2 +from cv_bridge import CvBridge +import numpy as np +import rclpy +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CameraInfo, Image, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger +from tf2_msgs.msg import TFMessage + +from .acquisition import ( + ContinuousSweepCollector, + Observation, + PointCollector, + StateSample, + TagQuality, + aggregate_sweep_observations, + interpolate_state_u8, + tag_quality_is_valid, +) +from .core import ( + BASELINE_COMMAND, + COMMAND_NAMES, + DIRECTION_DECREASING, + DIRECTION_INCREASING, + PAIR_IP, + PAIR_MCP, + PAIR_NAMES, + PAIR_ROOT, + PHASE_ROOT, + PHASE_TIP, + FitResult, + build_command, + create_final_payload, + fit_calibration_curves, + image_plane_tag_quaternion_xyzw, + maximum_non_target_drift_rad, + relative_quaternion_xyzw, + rotation_inlier_fraction, + rotation_rms_rad, + scan_targets, +) +from .storage import ( + append_jsonl, + atomic_write_json, + completed_scan_keys, + load_jsonl, +) + + +STATE_PREFLIGHT = "PREFLIGHT" +STATE_WAIT_ROOT = "WAIT_ROOT_CONFIRM" +STATE_SCAN_ROOT = "SCAN_ROOT" +STATE_WAIT_TIP = "WAIT_TIP_CONFIRM" +STATE_SCAN_TIP = "SCAN_TIP" +STATE_VALIDATING = "VALIDATING" +STATE_PAUSED = "PAUSED" +STATE_ABORTED = "ABORTED" +STATE_COMPLETE = "COMPLETE" + + +def _stamp_ns(stamp: Any) -> int: + return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + + +def _safe_name(value: str) -> str: + safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) + return safe or "UNSET" + + +class G20ThumbCalibrationNode(Node): + """Drive isolated G20 thumb sweeps and aggregate AprilTag rotations.""" + + def __init__(self) -> None: + super().__init__("g20_thumb_calibration") + self._declare_parameters() + self._load_parameters() + + self.session_dir.mkdir(parents=True, exist_ok=True) + self.raw_path = self.session_dir / "raw_samples.jsonl" + self.checkpoint_path = self.session_dir / "checkpoint.json" + self.validation_path = self.session_dir / "validation.json" + self.manifest_path = self.session_dir / "session_manifest.json" + self.final_path = self.session_dir / ( + f"g20_left_{_safe_name(self.serial_number)}_thumb_angle.json" + ) + + self._validate_existing_session() + self.records = load_jsonl(self.raw_path) + self.validation_records: list[dict[str, Any]] = [] + self.state = STATE_PREFLIGHT + self.state_reason = "waiting_for_camera_tags_and_sdk" + self.paused_from: str | None = None + self.current_phase: str | None = None + self.task_queue: deque[dict[str, Any]] = deque() + self.active_task: dict[str, Any] | None = None + self.pending_aggregate: dict[str, Any] | None = None + self.pending_sweep_observations: list[Observation] | None = None + self.fit: FitResult | None = None + self.validation_zero: dict[str, dict[str, list[float]]] = {} + self.validation_errors: list[float] = [] + + self.latest_state_u8: tuple[float, ...] = () + self.state_history: deque[StateSample] = deque(maxlen=600) + self.latest_hand_info: dict[str, Any] = {} + self.camera_intrinsics: dict[str, Any] = {} + self.camera_info_valid = False + self.camera_frame = "" + self.preflight_detection_flags: deque[bool] = deque( + maxlen=self.preflight_frames + ) + self.preflight_detection_times: deque[float] = deque( + maxlen=self.preflight_frames + ) + self.preflight_observations: deque[Observation] = deque( + maxlen=self.preflight_frames + ) + + self.quality_cache: dict[int, dict[str, TagQuality]] = {} + self.corner_cache: dict[int, dict[str, np.ndarray]] = {} + self.transform_cache: dict[int, dict[str, tuple[float, ...]]] = {} + self.emitted_stamps: set[int] = set() + self.latest_corners: dict[str, np.ndarray] = {} + + self.collector = PointCollector( + stable_frames=self.stable_frames, + capture_frames=self.capture_frames, + minimum_settle_seconds=self.minimum_settle_seconds, + maximum_stable_spread_rad=self.maximum_stable_spread_rad, + settle_timeout_seconds=self.settle_timeout_seconds, + capture_timeout_seconds=self.capture_timeout_seconds, + ) + self.sweep_collector = ContinuousSweepCollector( + endpoint_tolerance_u8=self.continuous_endpoint_tolerance_u8, + endpoint_hold_seconds=self.continuous_endpoint_hold_seconds, + timeout_seconds=self.continuous_timeout_seconds, + invalid_timeout_seconds=self.continuous_invalid_timeout_seconds, + minimum_valid_frames=self.continuous_minimum_valid_frames, + minimum_state_span_u8=self.continuous_minimum_state_span_u8, + ) + + self.command_publisher = self.create_publisher( + JointState, self.command_topic, 1 + ) + self.status_publisher = self.create_publisher( + String, "~/status", 10 + ) + self.debug_publisher = None + self.last_debug_publish = 0.0 + if self.publish_debug_image: + self.debug_publisher = self.create_publisher( + Image, "~/debug_image", qos_profile_sensor_data + ) + self.bridge = CvBridge() + + self.create_subscription( + JointState, self.state_topic, self._state_callback, 10 + ) + self.create_subscription( + String, self.info_topic, self._info_callback, 10 + ) + self.create_subscription( + CameraInfo, self.camera_info_topic, self._camera_info_callback, 10 + ) + self.create_subscription( + AprilTagDetectionArray, + self.detections_topic, + self._detections_callback, + 10, + ) + self.create_subscription(TFMessage, self.tf_topic, self._tf_callback, 50) + if self.publish_debug_image: + self.create_subscription( + Image, + self.image_topic, + self._image_callback, + qos_profile_sensor_data, + ) + + self.create_service( + Trigger, "~/start", self._start_callback + ) + self.create_service( + Trigger, "~/confirm_root_full_range", self._confirm_root_callback + ) + self.create_service( + Trigger, "~/confirm_tip_full_range", self._confirm_tip_callback + ) + self.create_service(Trigger, "~/pause", self._pause_callback) + self.create_service(Trigger, "~/resume", self._resume_callback) + self.create_service(Trigger, "~/abort", self._abort_callback) + + self.last_status_publish = 0.0 + self.timer = self.create_timer(0.05, self._timer_callback) + self._write_manifest() + self.get_logger().info( + f"Calibration session: {self.session_dir}; commands_enabled={self.commands_enabled}" + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("serial_number", "UNSET") + self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("commands_enabled", True) + self.declare_parameter("calibration_speed", 15) + self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") + self.declare_parameter("state_topic", "/g20/cb_left_hand_state") + self.declare_parameter("info_topic", "/g20/cb_left_hand_info") + self.declare_parameter( + "camera_info_topic", "/camera/camera/color/camera_info" + ) + self.declare_parameter( + "image_topic", "/camera/camera/color/image_rect" + ) + self.declare_parameter("detections_topic", "/apriltag/detections") + self.declare_parameter("tf_topic", "/tf") + self.declare_parameter("angle_estimation_mode", "image_plane_2d") + self.declare_parameter("camera_serial_number", "") + self.declare_parameter("rosbag_path", "") + self.declare_parameter("publish_debug_image", False) + self.declare_parameter("debug_max_rate_hz", 10.0) + self.declare_parameter("debug_scale", 0.5) + self.declare_parameter("tag_roles", ["t0", "t3", "t4", "t5"]) + self.declare_parameter("tag_ids", [0, 1, 2, 3]) + self.declare_parameter("tag_frames", ["tag_t0", "tag_t3", "tag_t4", "tag_t5"]) + self.declare_parameter("tag_sizes_m", [0.02, 0.02, 0.02, 0.02]) + self.declare_parameter("repetitions", 1) + self.declare_parameter("command_step", 8) + self.declare_parameter("scan_mode", "continuous") + self.declare_parameter("continuous_motion_mode", "endpoint") + self.declare_parameter("auto_start_tip", True) + self.declare_parameter("maximum_state_image_skew_ms", 150.0) + self.declare_parameter("continuous_endpoint_tolerance_u8", 2.0) + self.declare_parameter("continuous_endpoint_hold_seconds", 1.0) + self.declare_parameter("continuous_timeout_seconds", 90.0) + self.declare_parameter("continuous_invalid_timeout_seconds", 3.0) + self.declare_parameter("continuous_minimum_valid_frames", 40) + self.declare_parameter("continuous_minimum_state_span_u8", 240.0) + self.declare_parameter("continuous_minimum_bins", 32) + self.declare_parameter("continuous_maximum_bin_gap", 16) + self.declare_parameter("continuous_segment_minimum_seconds", 0.1) + self.declare_parameter("continuous_segment_timeout_seconds", 5.0) + self.declare_parameter("continuous_prepare_timeout_seconds", 30.0) + self.declare_parameter("preflight_frames", 150) + self.declare_parameter("minimum_detection_rate", 0.95) + self.declare_parameter("minimum_detection_hz", 15.0) + self.declare_parameter("maximum_hamming", 0) + self.declare_parameter("minimum_decision_margin", 30.0) + self.declare_parameter("minimum_edge_pixels", 40.0) + self.declare_parameter("maximum_static_std_deg", 0.5) + self.declare_parameter("pose_outlier_threshold_deg", 5.0) + self.declare_parameter("minimum_pose_inlier_rate", 0.95) + self.declare_parameter("stable_frames", 5) + self.declare_parameter("capture_frames", 8) + self.declare_parameter("minimum_settle_seconds", 0.4) + self.declare_parameter("maximum_stable_spread_deg", 0.3) + self.declare_parameter("settle_timeout_seconds", 5.0) + self.declare_parameter("capture_timeout_seconds", 5.0) + self.declare_parameter("validation_command_count", 5) + self.declare_parameter("validation_approach_minimum_seconds", 0.2) + self.declare_parameter("validation_approach_timeout_seconds", 10.0) + self.declare_parameter("validation_position_tolerance_u8", 2.0) + self.declare_parameter("validation_seed", 20260727) + self.declare_parameter("maximum_validation_mae_deg", 2.0) + self.declare_parameter("maximum_validation_p95_deg", 3.0) + self.declare_parameter("maximum_coupling_drift_deg", 2.0) + self.declare_parameter("minimum_ip_coupling_r_squared", 0.98) + self.declare_parameter("maximum_monotonic_correction_deg", 2.0) + self.declare_parameter("maximum_hysteresis_deg", 5.0) + + def _load_parameters(self) -> None: + value = lambda name: self.get_parameter(name).value + self.serial_number = str(value("serial_number")) + if self.serial_number == "UNSET": + raise ValueError("serial_number must be set for a hardware calibration") + self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + self.commands_enabled = bool(value("commands_enabled")) + self.calibration_speed = int(value("calibration_speed")) + self.command_topic = str(value("command_topic")) + self.state_topic = str(value("state_topic")) + self.info_topic = str(value("info_topic")) + self.camera_info_topic = str(value("camera_info_topic")) + self.image_topic = str(value("image_topic")) + self.detections_topic = str(value("detections_topic")) + self.tf_topic = str(value("tf_topic")) + self.angle_estimation_mode = str(value("angle_estimation_mode")) + if self.angle_estimation_mode not in {"image_plane_2d", "pnp_3d"}: + raise ValueError( + "angle_estimation_mode must be image_plane_2d or pnp_3d" + ) + self.camera_serial_number = str(value("camera_serial_number")) + self.rosbag_path = str(value("rosbag_path")) + self.publish_debug_image = bool(value("publish_debug_image")) + self.debug_max_rate_hz = float(value("debug_max_rate_hz")) + self.debug_scale = float(value("debug_scale")) + if self.debug_max_rate_hz <= 0.0: + raise ValueError("debug_max_rate_hz must be positive") + if not 0.1 <= self.debug_scale <= 1.0: + raise ValueError("debug_scale must be in [0.1, 1.0]") + + roles = [str(item) for item in value("tag_roles")] + ids = [int(item) for item in value("tag_ids")] + frames = [str(item).lstrip("/") for item in value("tag_frames")] + sizes = [float(item) for item in value("tag_sizes_m")] + if len(roles) != 4 or set(roles) != {"t0", "t3", "t4", "t5"}: + raise ValueError("tag_roles must contain t0, t3, t4, t5") + if not (len(ids) == len(frames) == len(sizes) == 4): + raise ValueError("tag_ids, tag_frames and tag_sizes_m must have length 4") + if len(set(ids)) != 4 or len(set(frames)) != 4: + raise ValueError("tag IDs and frames must be unique") + if any(size <= 0.0 for size in sizes): + raise ValueError("all tag sizes must be measured positive values") + self.role_by_id = dict(zip(ids, roles)) + self.role_by_frame = dict(zip(frames, roles)) + self.tag_config = { + role: {"id": tag_id, "frame": frame, "size_m": size} + for role, tag_id, frame, size in zip(roles, ids, frames, sizes) + } + + self.repetitions = int(value("repetitions")) + self.command_step = int(value("command_step")) + self.scan_mode = str(value("scan_mode")) + self.continuous_motion_mode = str(value("continuous_motion_mode")) + self.auto_start_tip = bool(value("auto_start_tip")) + self.maximum_state_image_skew_ns = int( + float(value("maximum_state_image_skew_ms")) * 1_000_000.0 + ) + self.continuous_endpoint_tolerance_u8 = float( + value("continuous_endpoint_tolerance_u8") + ) + self.continuous_endpoint_hold_seconds = float( + value("continuous_endpoint_hold_seconds") + ) + self.continuous_timeout_seconds = float( + value("continuous_timeout_seconds") + ) + self.continuous_invalid_timeout_seconds = float( + value("continuous_invalid_timeout_seconds") + ) + self.continuous_minimum_valid_frames = int( + value("continuous_minimum_valid_frames") + ) + self.continuous_minimum_state_span_u8 = float( + value("continuous_minimum_state_span_u8") + ) + self.continuous_minimum_bins = int(value("continuous_minimum_bins")) + self.continuous_maximum_bin_gap = int( + value("continuous_maximum_bin_gap") + ) + self.continuous_segment_minimum_seconds = float( + value("continuous_segment_minimum_seconds") + ) + self.continuous_segment_timeout_seconds = float( + value("continuous_segment_timeout_seconds") + ) + self.continuous_prepare_timeout_seconds = float( + value("continuous_prepare_timeout_seconds") + ) + self.preflight_frames = int(value("preflight_frames")) + self.minimum_detection_rate = float(value("minimum_detection_rate")) + self.minimum_detection_hz = float(value("minimum_detection_hz")) + self.maximum_hamming = int(value("maximum_hamming")) + self.minimum_decision_margin = float(value("minimum_decision_margin")) + self.minimum_edge_pixels = float(value("minimum_edge_pixels")) + self.maximum_static_std_rad = math.radians( + float(value("maximum_static_std_deg")) + ) + self.pose_outlier_threshold_rad = math.radians( + float(value("pose_outlier_threshold_deg")) + ) + self.minimum_pose_inlier_rate = float( + value("minimum_pose_inlier_rate") + ) + self.stable_frames = int(value("stable_frames")) + self.capture_frames = int(value("capture_frames")) + self.minimum_settle_seconds = float(value("minimum_settle_seconds")) + self.maximum_stable_spread_rad = math.radians( + float(value("maximum_stable_spread_deg")) + ) + self.settle_timeout_seconds = float(value("settle_timeout_seconds")) + self.capture_timeout_seconds = float(value("capture_timeout_seconds")) + self.validation_command_count = int(value("validation_command_count")) + self.validation_approach_minimum_seconds = float( + value("validation_approach_minimum_seconds") + ) + self.validation_approach_timeout_seconds = float( + value("validation_approach_timeout_seconds") + ) + self.validation_position_tolerance_u8 = float( + value("validation_position_tolerance_u8") + ) + self.validation_seed = int(value("validation_seed")) + self.maximum_validation_mae_rad = math.radians( + float(value("maximum_validation_mae_deg")) + ) + self.maximum_validation_p95_rad = math.radians( + float(value("maximum_validation_p95_deg")) + ) + self.maximum_coupling_drift_rad = math.radians( + float(value("maximum_coupling_drift_deg")) + ) + self.minimum_ip_coupling_r_squared = float( + value("minimum_ip_coupling_r_squared") + ) + self.maximum_monotonic_correction_rad = math.radians( + float(value("maximum_monotonic_correction_deg")) + ) + self.maximum_hysteresis_rad = math.radians( + float(value("maximum_hysteresis_deg")) + ) + if self.repetitions < 1: + raise ValueError("repetitions must be positive") + if not 0 <= self.calibration_speed <= 255: + raise ValueError("calibration_speed must be in [0, 255]") + if self.scan_mode not in {"continuous", "point"}: + raise ValueError("scan_mode must be continuous or point") + if self.continuous_motion_mode not in {"endpoint", "paced"}: + raise ValueError( + "continuous_motion_mode must be endpoint or paced" + ) + if not 1 <= self.command_step <= 255: + raise ValueError("command_step must be in [1, 255]") + if self.maximum_state_image_skew_ns < 0: + raise ValueError("maximum_state_image_skew_ms must be non-negative") + if self.continuous_minimum_bins < 3: + raise ValueError("continuous_minimum_bins must be at least 3") + if self.continuous_maximum_bin_gap < 1: + raise ValueError("continuous_maximum_bin_gap must be positive") + if self.continuous_segment_minimum_seconds < 0.0: + raise ValueError( + "continuous_segment_minimum_seconds must be non-negative" + ) + if self.continuous_segment_timeout_seconds <= 0.0: + raise ValueError( + "continuous_segment_timeout_seconds must be positive" + ) + if self.continuous_prepare_timeout_seconds <= 0.0: + raise ValueError( + "continuous_prepare_timeout_seconds must be positive" + ) + if self.preflight_frames < 3: + raise ValueError("preflight_frames must be at least 3") + if not 0.0 <= self.minimum_detection_rate <= 1.0: + raise ValueError("minimum_detection_rate must be in [0, 1]") + if self.minimum_detection_hz <= 0.0: + raise ValueError("minimum_detection_hz must be positive") + if self.pose_outlier_threshold_rad <= 0.0: + raise ValueError("pose_outlier_threshold_deg must be positive") + if not 0.0 <= self.minimum_pose_inlier_rate <= 1.0: + raise ValueError("minimum_pose_inlier_rate must be in [0, 1]") + if not 1 <= self.validation_command_count <= 254: + raise ValueError("validation_command_count must be in [1, 254]") + if self.validation_approach_minimum_seconds < 0.0: + raise ValueError( + "validation_approach_minimum_seconds must be non-negative" + ) + if self.validation_approach_timeout_seconds <= 0.0: + raise ValueError( + "validation_approach_timeout_seconds must be positive" + ) + if self.validation_position_tolerance_u8 < 0.0: + raise ValueError( + "validation_position_tolerance_u8 must be non-negative" + ) + + def _camera_info_callback(self, message: CameraInfo) -> None: + self.camera_info_valid = ( + message.width > 0 + and message.height > 0 + and len(message.p) == 12 + and float(message.p[0]) > 0.0 + and float(message.p[5]) > 0.0 + ) + self.camera_frame = message.header.frame_id + self.camera_intrinsics = { + "frame_id": message.header.frame_id, + "width": int(message.width), + "height": int(message.height), + "distortion_model": message.distortion_model, + "d": [float(value) for value in message.d], + "k": [float(value) for value in message.k], + "p": [float(value) for value in message.p], + } + + def _state_callback(self, message: JointState) -> None: + if len(message.position) != 20: + return + if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): + lookup = dict(zip(message.name, message.position)) + self.latest_state_u8 = tuple( + float(lookup[name]) for name in COMMAND_NAMES + ) + else: + self.latest_state_u8 = tuple(float(value) for value in message.position) + stamp_ns = _stamp_ns(message.header.stamp) + if stamp_ns <= 0: + stamp_ns = int(self.get_clock().now().nanoseconds) + if not self.state_history or stamp_ns > self.state_history[-1].stamp_ns: + self.state_history.append( + StateSample( + stamp_ns=stamp_ns, + position_u8=self.latest_state_u8, + ) + ) + + def _info_callback(self, message: String) -> None: + try: + value = json.loads(message.data) + except json.JSONDecodeError: + return + if isinstance(value, dict): + self.latest_hand_info = value + + def _detections_callback(self, message: AprilTagDetectionArray) -> None: + stamp = _stamp_ns(message.header.stamp) + qualities: dict[str, TagQuality] = {} + latest_corners: dict[str, np.ndarray] = {} + for detection in message.detections: + role = self.role_by_id.get(int(detection.id)) + if role is None: + continue + corners = np.asarray( + [[float(point.x), float(point.y)] for point in detection.corners], + dtype=float, + ) + if corners.shape != (4, 2): + continue + edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) + qualities[role] = TagQuality( + hamming=int(detection.hamming), + decision_margin=float(detection.decision_margin), + edge_pixels=float(np.mean(edges)), + ) + latest_corners[role] = corners + self.latest_corners = latest_corners + self.quality_cache[stamp] = qualities + self.corner_cache[stamp] = latest_corners + all_good = set(qualities) == set(self.tag_config) and all( + self._quality_is_valid(quality) for quality in qualities.values() + ) + self.preflight_detection_flags.append(all_good) + self.preflight_detection_times.append(time.monotonic()) + if not all_good and self.collector.active: + self.collector.mark_invalid_frame() + self._try_emit_observation(stamp) + self._trim_caches() + + def _tf_callback(self, message: TFMessage) -> None: + touched: set[int] = set() + for transform in message.transforms: + role = self.role_by_frame.get(transform.child_frame_id.lstrip("/")) + if role is None: + continue + stamp = _stamp_ns(transform.header.stamp) + rotation = transform.transform.rotation + quaternion = ( + float(rotation.x), + float(rotation.y), + float(rotation.z), + float(rotation.w), + ) + self.transform_cache.setdefault(stamp, {})[role] = quaternion + touched.add(stamp) + for stamp in touched: + self._try_emit_observation(stamp) + self._trim_caches() + + def _quality_is_valid(self, quality: TagQuality) -> bool: + return tag_quality_is_valid( + quality, + maximum_hamming=self.maximum_hamming, + minimum_decision_margin=self.minimum_decision_margin, + minimum_edge_pixels=self.minimum_edge_pixels, + ) + + def _try_emit_observation(self, stamp: int) -> None: + if stamp in self.emitted_stamps: + return + quality = self.quality_cache.get(stamp) + required = set(self.tag_config) + if quality is None: + return + if set(quality) != required: + return + if not all(self._quality_is_valid(value) for value in quality.values()): + return + if self.angle_estimation_mode == "image_plane_2d": + corners = self.corner_cache.get(stamp) + if corners is None or set(corners) != required: + return + transforms = { + role: image_plane_tag_quaternion_xyzw(points) + for role, points in corners.items() + } + else: + transforms = self.transform_cache.get(stamp) + if transforms is None or set(transforms) != required: + return + relative = { + PAIR_ROOT: relative_quaternion_xyzw( + transforms["t0"], transforms["t3"] + ), + PAIR_MCP: relative_quaternion_xyzw( + transforms["t3"], transforms["t4"] + ), + PAIR_IP: relative_quaternion_xyzw( + transforms["t4"], transforms["t5"] + ), + } + matched_state = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched_state is None: + state_u8 = self.latest_state_u8 + state_sync_error_ns = None + state_stamp_ns = None + else: + state_u8, state_sync_error_ns = matched_state + state_stamp_ns = stamp + observation = Observation( + stamp_ns=stamp, + received_at=time.monotonic(), + relative_quaternion_xyzw=relative, + tag_quality=quality, + state_u8=state_u8, + state_stamp_ns=state_stamp_ns, + state_sync_error_ns=state_sync_error_ns, + ) + self.emitted_stamps.add(stamp) + self.preflight_observations.append(observation) + if self.collector.active: + result = self.collector.add(observation, time.monotonic()) + if result is not None: + self.pending_aggregate = result + if self.sweep_collector.active: + result = self.sweep_collector.add(observation, time.monotonic()) + if result is not None: + self.pending_sweep_observations = result + + def _trim_caches(self) -> None: + all_stamps = sorted( + set(self.quality_cache) + | set(self.corner_cache) + | set(self.transform_cache) + ) + for stamp in all_stamps[:-300]: + self.quality_cache.pop(stamp, None) + self.corner_cache.pop(stamp, None) + self.transform_cache.pop(stamp, None) + self.emitted_stamps.discard(stamp) + + def _image_callback(self, message: Image) -> None: + if ( + self.debug_publisher is None + or self.debug_publisher.get_subscription_count() < 1 + ): + return + now = time.monotonic() + if now - self.last_debug_publish < 1.0 / self.debug_max_rate_hz: + return + self.last_debug_publish = now + try: + image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8") + except Exception as error: + self.get_logger().warning(f"debug image conversion failed: {error}") + return + if self.debug_scale != 1.0: + image = cv2.resize( + image, + None, + fx=self.debug_scale, + fy=self.debug_scale, + interpolation=cv2.INTER_AREA, + ) + colors = { + "t0": (0, 200, 0), + "t3": (0, 128, 255), + "t4": (200, 0, 200), + "t5": (0, 220, 255), + } + for role, corners in self.latest_corners.items(): + points = np.rint(corners * self.debug_scale).astype(np.int32) + cv2.polylines(image, [points], True, colors[role], 2) + origin = tuple(points[0]) + cv2.putText( + image, + role.upper(), + origin, + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + colors[role], + 2, + ) + cv2.putText( + image, + f"{self.state}: {self.state_reason}", + (20, 32), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (0, 0, 255) if self.state in {STATE_PAUSED, STATE_ABORTED} else (0, 255, 0), + 2, + ) + output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + output.header = message.header + self.debug_publisher.publish(output) + + def _timer_callback(self) -> None: + now = time.monotonic() + self.collector.poll(now) + self.sweep_collector.poll(now) + if self.collector.state == "failed" and self.state not in { + STATE_PAUSED, + STATE_ABORTED, + }: + self._pause(f"point_capture_failed:{self.collector.reason}") + if self.sweep_collector.state == "failed" and self.state not in { + STATE_PAUSED, + STATE_ABORTED, + }: + self._pause( + f"continuous_sweep_failed:{self.sweep_collector.reason}" + ) + + if self.pending_aggregate is not None: + aggregate = self.pending_aggregate + self.pending_aggregate = None + self._complete_active_task(aggregate) + if self.pending_sweep_observations is not None: + observations = self.pending_sweep_observations + self.pending_sweep_observations = None + self._complete_continuous_sweep(observations) + + if ( + self.active_task is not None + and self.active_task.get("kind") == "continuous_sweep" + and self.active_task.get("stage") == "prepare_move" + ): + self._poll_continuous_prepare(now) + + if ( + self.active_task is not None + and self.active_task.get("kind") == "continuous_sweep" + and self.active_task.get("stage") == "sweep" + and self.active_task.get("motion_mode") == "paced" + and self.sweep_collector.active + ): + self._poll_continuous_segment(now) + + if ( + self.state == STATE_VALIDATING + and self.active_task is not None + and self.active_task.get("kind") == "validation" + and self.active_task.get("stage") == "approach" + ): + self._poll_validation_approach(now) + + if self.state == STATE_PREFLIGHT: + passed, reason = self._preflight_status() + self.state_reason = reason + if passed: + self._select_post_preflight_state() + elif self.state in {STATE_SCAN_ROOT, STATE_SCAN_TIP, STATE_VALIDATING}: + if ( + self.active_task is None + and not self.collector.active + and not self.sweep_collector.active + ): + self._start_next_task() + + if now - self.last_status_publish >= 0.5: + self._publish_status() + self.last_status_publish = now + + def _preflight_status(self) -> tuple[bool, str]: + if not self.camera_info_valid: + return False, "camera_info_not_ready" + if len(self.latest_state_u8) != 20: + return False, "g20_state_not_ready" + if self.command_publisher.get_subscription_count() < 1: + return False, "g20_sdk_not_subscribed" + if self._has_other_command_publishers(): + return False, "another_command_publisher_exists" + if len(self.preflight_detection_flags) < self.preflight_frames: + return ( + False, + "collecting_detection_preflight:" + f"{len(self.preflight_detection_flags)}/{self.preflight_frames}", + ) + detection_rate = float(np.mean(self.preflight_detection_flags)) + if detection_rate < self.minimum_detection_rate: + return False, f"detection_rate_too_low:{detection_rate:.3f}" + detection_times = list(self.preflight_detection_times) + duration = detection_times[-1] - detection_times[0] + detection_hz = ( + (len(detection_times) - 1) / duration + if duration > 0.0 + else 0.0 + ) + if detection_hz < self.minimum_detection_hz: + return False, f"detection_hz_too_low:{detection_hz:.2f}" + if len(self.preflight_observations) < self.preflight_frames: + return ( + False, + "collecting_static_preflight:" + f"{len(self.preflight_observations)}/{self.preflight_frames}", + ) + observations = list(self.preflight_observations) + for pair in PAIR_NAMES: + quaternions = [ + observation.relative_quaternion_xyzw[pair] + for observation in observations + ] + inlier_rate = rotation_inlier_fraction( + quaternions, + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + if inlier_rate < self.minimum_pose_inlier_rate: + return False, ( + f"{pair}_pose_inlier_rate_too_low:{inlier_rate:.3f}" + ) + rms = rotation_rms_rad( + quaternions, + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + if rms > self.maximum_static_std_rad: + return False, f"{pair}_static_rms_too_large:{rms:.6f}" + return True, "preflight_passed" + + def _has_other_command_publishers(self) -> bool: + try: + publishers = self.get_publishers_info_by_topic(self.command_topic) + except Exception: + return True + return any( + endpoint.node_name != self.get_name() + or endpoint.node_namespace != self.get_namespace() + for endpoint in publishers + ) + + def _select_post_preflight_state(self) -> None: + observations = list(self.preflight_observations) + self._write_manifest( + { + "preflight": { + "passed": True, + "detection_rate": float( + np.mean(self.preflight_detection_flags) + ), + "detection_hz": ( + (len(self.preflight_detection_times) - 1) + / ( + self.preflight_detection_times[-1] + - self.preflight_detection_times[0] + ) + ), + "static_rms_rad": { + pair: rotation_rms_rad( + [ + observation.relative_quaternion_xyzw[pair] + for observation in observations + ], + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + for pair in PAIR_NAMES + }, + "pose_inlier_rate": { + pair: rotation_inlier_fraction( + [ + observation.relative_quaternion_xyzw[pair] + for observation in observations + ], + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + for pair in PAIR_NAMES + }, + } + } + ) + if self.final_path.exists(): + self.state = STATE_COMPLETE + self.state_reason = "existing_final_result" + elif not self._phase_complete(PHASE_ROOT): + self.state = STATE_WAIT_ROOT + self.state_reason = "call_start" + elif not self._phase_complete(PHASE_TIP): + self.state = STATE_WAIT_TIP + self.state_reason = "call_start_to_resume_tip" + else: + self._begin_validation() + + def _phase_complete(self, phase: str) -> bool: + if self.scan_mode == "continuous": + return all( + self._continuous_direction_complete(phase, direction) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + completed = completed_scan_keys(self.records) + return all( + (phase, cycle, direction, command) in completed + for cycle, direction, command in scan_targets( + self.repetitions, self.command_step + ) + ) + + def _continuous_direction_complete( + self, phase: str, direction: str + ) -> bool: + commands = sorted( + { + int(record["command_u8"]) + for record in self.records + if record.get("kind", "sample") == "sample" + and record.get("scan_mode") == "continuous" + and record.get("phase") == phase + and record.get("direction") == direction + } + ) + if ( + len(commands) < self.continuous_minimum_bins + or not commands + or commands[0] != 0 + or commands[-1] != 255 + ): + return False + return max(np.diff(commands), default=0) <= self.continuous_maximum_bin_gap + + def _start_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state not in {STATE_WAIT_ROOT, STATE_WAIT_TIP}: + response.success = False + response.message = f"current state is {self.state}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled=false; restart for hardware scan" + return response + phase = PHASE_ROOT if self.state == STATE_WAIT_ROOT else PHASE_TIP + self._begin_scan(phase) + response.success = True + response.message = ( + f"{self.scan_mode} calibration armed; root, tip and validation " + "run automatically" + if phase == PHASE_ROOT and self.auto_start_tip + else f"{phase} scan armed" + ) + return response + + def _confirm_root_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_WAIT_ROOT: + response.success = False + response.message = f"current state is {self.state}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled=false; restart for hardware scan" + return response + self._begin_scan(PHASE_ROOT) + response.success = True + response.message = "root full-range scan armed" + return response + + def _confirm_tip_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_WAIT_TIP: + response.success = False + response.message = f"current state is {self.state}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled=false; restart for hardware scan" + return response + self._begin_scan(PHASE_TIP) + response.success = True + response.message = "tip full-range scan armed" + return response + + def _pause_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state not in {STATE_SCAN_ROOT, STATE_SCAN_TIP, STATE_VALIDATING}: + response.success = False + response.message = f"cannot pause from {self.state}" + return response + self._pause("operator_pause") + response.success = True + response.message = "calibration paused; current command is held" + return response + + def _resume_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_PAUSED or self.paused_from is None: + response.success = False + response.message = "calibration is not resumably paused" + return response + self.state = self.paused_from + self.state_reason = "resuming_current_task" + self.paused_from = None + if ( + self.active_task is not None + and self.active_task.get("kind") == "continuous_sweep" + ): + # An end-to-end command cannot resume halfway with correct + # direction/history. Return to its start endpoint and repeat only + # the interrupted sweep. + self.active_task["stage"] = "prepare" + self.active_task["command_u8"] = int( + self.active_task["start_u8"] + ) + self.active_task.pop("start_aggregate", None) + self._restart_active_task() + response.success = True + response.message = "calibration resumed" + return response + + def _abort_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + self.collector.state = "idle" + self.sweep_collector.state = "idle" + self.pending_aggregate = None + self.pending_sweep_observations = None + self.state = STATE_ABORTED + self.state_reason = "operator_abort_current_command_held" + self._write_checkpoint() + response.success = True + response.message = "aborted without sending a return command" + return response + + def _pause(self, reason: str) -> None: + if self.state == STATE_PAUSED: + return + self.paused_from = self.state + self.collector.state = "idle" + self.sweep_collector.state = "idle" + self.pending_aggregate = None + self.pending_sweep_observations = None + self.state = STATE_PAUSED + self.state_reason = reason + self._write_checkpoint() + + def _begin_scan(self, phase: str) -> None: + if self.scan_mode == "continuous": + tasks: list[dict[str, Any]] = [] + for direction, start, target in ( + (DIRECTION_DECREASING, 255, 0), + (DIRECTION_INCREASING, 0, 255), + ): + if self._continuous_direction_complete(phase, direction): + continue + tasks.append( + { + "kind": "continuous_sweep", + "scan_mode": "continuous", + "motion_mode": self.continuous_motion_mode, + "phase": phase, + "cycle": 0, + "direction": direction, + "start_u8": start, + "target_u8": target, + "command_u8": start, + "stage": "prepare", + } + ) + self.task_queue = deque(tasks) + else: + completed = completed_scan_keys(self.records) + self.task_queue = deque( + { + "kind": "sample", + "scan_mode": "point", + "phase": phase, + "cycle": cycle, + "direction": direction, + "command_u8": command, + } + for cycle, direction, command in scan_targets( + self.repetitions, self.command_step + ) + if (phase, cycle, direction, command) not in completed + ) + self.current_phase = phase + self.active_task = None + self.state = STATE_SCAN_ROOT if phase == PHASE_ROOT else STATE_SCAN_TIP + self.state_reason = "scan_armed" + self._write_checkpoint() + + def _start_next_task(self) -> None: + if not self.task_queue: + if self.state == STATE_SCAN_ROOT: + if self.auto_start_tip: + self._begin_scan(PHASE_TIP) + else: + self.state = STATE_WAIT_TIP + self.state_reason = "root_complete_call_start_for_tip" + self.current_phase = None + self._write_checkpoint() + elif self.state == STATE_SCAN_TIP: + self.current_phase = None + self._begin_validation() + elif self.state == STATE_VALIDATING: + self._finalize() + return + self.active_task = self.task_queue.popleft() + self.active_task.setdefault("stage", "target") + self._restart_active_task() + + def _restart_active_task(self) -> None: + if self.active_task is None: + return + task = self.active_task + if task.get("kind") == "continuous_sweep": + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + if task.get("stage") == "prepare": + command = int(task["start_u8"]) + task["command_u8"] = command + self._publish_hand_command( + build_command(motor_index, command) + ) + task["stage"] = "prepare_move" + task["prepare_started_at"] = time.monotonic() + self.collector.state = "idle" + self.state_reason = ( + f"{task['phase']}:continuous:prepare_move:u={command}" + ) + return + if task.get("stage") != "sweep": + raise RuntimeError( + f"unknown continuous stage {task.get('stage')}" + ) + now = time.monotonic() + self.sweep_collector.start( + now, + motor_index=motor_index, + start_u8=int(task["start_u8"]), + target_u8=int(task["target_u8"]), + ) + if task.get("motion_mode") == "endpoint": + command = int(task["target_u8"]) + task["command_u8"] = command + task.pop("segment_targets_u8", None) + task.pop("segment_index", None) + task.pop("segment_started_at", None) + self._publish_hand_command( + build_command(motor_index, command) + ) + self.state_reason = ( + f"{task['phase']}:continuous_endpoint:" + f"{task['direction']}:" + f"{task['start_u8']}->{task['target_u8']}" + ) + return + direction_targets = [ + command + for _, direction, command in scan_targets( + 1, self.command_step + ) + if direction == task["direction"] + ] + if not direction_targets or int(direction_targets[0]) != int( + task["start_u8"] + ): + raise RuntimeError("continuous segment grid has wrong start") + task["segment_targets_u8"] = [ + int(command) for command in direction_targets[1:] + ] + task["segment_index"] = 0 + command = int(task["segment_targets_u8"][0]) + task["command_u8"] = command + task["segment_started_at"] = now + self._publish_hand_command(build_command(motor_index, command)) + self.state_reason = ( + f"{task['phase']}:continuous_paced:{task['direction']}:" + f"{task['start_u8']}->{task['target_u8']}" + ) + return + + command = int(task["command_u8"]) + if task.get("kind") == "validation" and task.get("stage") == "approach": + command = int(task["approach_u8"]) + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + self._publish_hand_command(build_command(motor_index, command)) + if task.get("kind") == "validation" and task.get("stage") == "approach": + task["approach_started_at"] = time.monotonic() + self.collector.state = "idle" + self.state_reason = ( + f"{task['phase']}:validation:approach_wait:u={command}" + ) + return + self.collector.start(time.monotonic()) + self.state_reason = ( + f"{task['phase']}:{task.get('kind')}:{task.get('stage')}:u={command}" + ) + + def _publish_hand_command(self, values: list[int]) -> None: + if not self.commands_enabled: + self.get_logger().warning( + f"dry-run: suppressed hardware command {values}" + ) + return + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = list(COMMAND_NAMES) + message.position = [float(value) for value in values] + self.command_publisher.publish(message) + + def _complete_active_task(self, aggregate: dict[str, Any]) -> None: + if self.active_task is None: + return + task = self.active_task + if task.get("kind") == "continuous_sweep": + if task.get("stage") != "prepare_capture": + raise RuntimeError( + "continuous point capture is only for prepare_capture" + ) + task["start_aggregate"] = aggregate + task["stage"] = "sweep" + task.pop("prepare_started_at", None) + self.collector.state = "idle" + self._restart_active_task() + return + if task.get("kind") == "sample": + record = { + "kind": "sample", + "scan_mode": task.get("scan_mode", "point"), + "phase": task["phase"], + "cycle": int(task["cycle"]), + "direction": task["direction"], + "command_u8": int(task["command_u8"]), + **aggregate, + } + append_jsonl(self.raw_path, record) + self.records.append(record) + self.active_task = None + self.collector.state = "idle" + self._write_checkpoint() + return + + if task.get("kind") != "validation": + raise RuntimeError(f"unknown task kind {task.get('kind')}") + if task.get("stage") == "approach": + raise RuntimeError("validation approach must not capture images") + if task.get("zero_reference"): + self.validation_zero[task["phase"]] = aggregate[ + "relative_quaternion_xyzw" + ] + else: + self._record_validation(task, aggregate) + self.active_task = None + self.collector.state = "idle" + self._write_checkpoint() + + def _poll_validation_approach(self, now: float) -> None: + task = self.active_task + if task is None: + return + started = float(task.get("approach_started_at", now)) + elapsed = float(now) - started + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + at_target = ( + len(self.latest_state_u8) == 20 + and abs( + float(self.latest_state_u8[motor_index]) + - float(task["approach_u8"]) + ) + <= self.validation_position_tolerance_u8 + ) + if ( + elapsed >= self.validation_approach_minimum_seconds + and at_target + ): + task["stage"] = "target" + task.pop("approach_started_at", None) + self._restart_active_task() + elif elapsed > self.validation_approach_timeout_seconds: + self._pause("validation_approach_timeout") + + def _poll_continuous_prepare(self, now: float) -> None: + task = self.active_task + if task is None: + return + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + command = int(task["start_u8"]) + elapsed = float(now) - float(task.get("prepare_started_at", now)) + at_target = ( + len(self.latest_state_u8) == 20 + and abs(float(self.latest_state_u8[motor_index]) - command) + <= self.continuous_endpoint_tolerance_u8 + ) + if at_target: + task["stage"] = "prepare_capture" + self.collector.start(float(now)) + self.state_reason = ( + f"{task['phase']}:continuous:prepare_capture:u={command}" + ) + elif elapsed > self.continuous_prepare_timeout_seconds: + actual = ( + float(self.latest_state_u8[motor_index]) + if len(self.latest_state_u8) == 20 + else float("nan") + ) + self._pause( + "continuous_prepare_timeout:" + f"target={command},actual={actual:.1f}" + ) + + def _poll_continuous_segment(self, now: float) -> None: + task = self.active_task + if task is None or not self.sweep_collector.observations: + return + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + command = int(task["command_u8"]) + observation = self.sweep_collector.observations[-1] + actual = float(observation.state_u8[motor_index]) + elapsed = float(now) - float(task.get("segment_started_at", now)) + at_target = ( + abs(actual - command) + <= self.continuous_endpoint_tolerance_u8 + ) + if ( + at_target + and elapsed >= self.continuous_segment_minimum_seconds + and command != int(task["target_u8"]) + ): + next_index = int(task["segment_index"]) + 1 + targets = task["segment_targets_u8"] + if next_index >= len(targets): + self._pause("continuous_segment_grid_exhausted") + return + next_command = int(targets[next_index]) + task["segment_index"] = next_index + task["command_u8"] = next_command + task["segment_started_at"] = float(now) + self._publish_hand_command( + build_command(motor_index, next_command) + ) + elif ( + not at_target + and elapsed > self.continuous_segment_timeout_seconds + ): + self._pause( + "continuous_segment_timeout:" + f"target={command},actual={actual:.1f}" + ) + + def _complete_continuous_sweep( + self, observations: list[Observation] + ) -> None: + task = self.active_task + if ( + task is None + or task.get("kind") != "continuous_sweep" + or task.get("stage") != "sweep" + ): + return + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + aggregates = aggregate_sweep_observations( + observations, + motor_index=motor_index, + start_u8=int(task["start_u8"]), + target_u8=int(task["target_u8"]), + endpoint_tolerance_u8=self.continuous_endpoint_tolerance_u8, + ) + start_command = int(task["start_u8"]) + start_aggregate = task.get("start_aggregate") + if isinstance(start_aggregate, dict): + aggregates[start_command] = start_aggregate + + commands = sorted(aggregates) + maximum_gap = max(np.diff(commands), default=0) + if ( + len(commands) < self.continuous_minimum_bins + or not commands + or commands[0] != 0 + or commands[-1] != 255 + or maximum_gap > self.continuous_maximum_bin_gap + ): + self.sweep_collector.state = "idle" + self._pause( + "continuous_sweep_insufficient_coverage:" + f"bins={len(commands)},max_gap={maximum_gap}" + ) + return + + for command in commands: + record = { + "kind": "sample", + "scan_mode": "continuous", + "phase": task["phase"], + "cycle": int(task["cycle"]), + "direction": task["direction"], + "command_u8": int(command), + "sweep_start_u8": int(task["start_u8"]), + "sweep_target_u8": int(task["target_u8"]), + **aggregates[command], + } + append_jsonl(self.raw_path, record) + self.records.append(record) + self.active_task = None + self.sweep_collector.state = "idle" + self._write_checkpoint() + + def _begin_validation(self) -> None: + try: + self.fit = fit_calibration_curves(self.records) + except Exception as error: + self._pause(f"curve_fit_failed:{error}") + return + generator = random.Random(self.validation_seed) + commands = sorted( + generator.sample(range(1, 255), self.validation_command_count) + ) + tasks: list[dict[str, Any]] = [] + for phase in (PHASE_ROOT, PHASE_TIP): + tasks.append( + { + "kind": "validation", + "phase": phase, + "direction": DIRECTION_DECREASING, + "command_u8": 255, + "zero_reference": True, + "stage": "target", + } + ) + for index, command in enumerate(commands): + direction = ( + DIRECTION_DECREASING + if index % 2 == 0 + else DIRECTION_INCREASING + ) + approach = ( + min(255, command + self.command_step) + if direction == DIRECTION_DECREASING + else max(0, command - self.command_step) + ) + tasks.append( + { + "kind": "validation", + "phase": phase, + "direction": direction, + "command_u8": command, + "approach_u8": approach, + "zero_reference": False, + "stage": "approach", + } + ) + tasks.append( + { + "kind": "validation", + "phase": phase, + "direction": DIRECTION_INCREASING, + "command_u8": 255, + "zero_reference": True, + "stage": "target", + } + ) + self.task_queue = deque(tasks) + self.active_task = None + self.validation_records = [] + self.validation_errors = [] + self.validation_zero = {} + self.state = STATE_VALIDATING + self.state_reason = "random_validation_started" + self._write_checkpoint() + + def _record_validation( + self, task: dict[str, Any], aggregate: dict[str, Any] + ) -> None: + if self.fit is None: + raise RuntimeError("validation requires fitted curves") + phase = str(task["phase"]) + direction = str(task["direction"]) + command = int(task["command_u8"]) + zero = self.validation_zero.get(phase) + if zero is None: + raise RuntimeError(f"missing validation zero for {phase}") + measurements: dict[str, float] = {} + predictions: dict[str, float] = {} + errors: dict[str, float] = {} + joint_names = ( + ("thumb_cmc_pitch",) + if phase == PHASE_ROOT + else ("thumb_mcp", "thumb_ip") + ) + for joint_name in joint_names: + pair = { + "thumb_cmc_pitch": PAIR_ROOT, + "thumb_mcp": PAIR_MCP, + "thumb_ip": PAIR_IP, + }[joint_name] + measurement = self.fit.measure_from_reference( + joint_name, + aggregate["relative_quaternion_xyzw"][pair], + zero[pair], + ) + prediction = float( + self.fit.joints[joint_name]["angle_rad"][command] + ) + error = measurement - prediction + measurements[joint_name] = measurement + predictions[joint_name] = prediction + errors[joint_name] = error + self.validation_errors.append(error) + record = { + "kind": "validation", + "phase": phase, + "direction": direction, + "command_u8": command, + "measurements_rad": measurements, + "predictions_rad": predictions, + "errors_rad": errors, + **aggregate, + } + self.validation_records.append(record) + append_jsonl(self.raw_path, record) + + def _finalize(self) -> None: + if self.fit is None: + self.fit = fit_calibration_curves(self.records) + errors = np.abs(np.asarray(self.validation_errors, dtype=float)) + mae = float(np.mean(errors)) if errors.size else float("inf") + p95 = float(np.percentile(errors, 95)) if errors.size else float("inf") + coupling_drift = maximum_non_target_drift_rad(self.records, self.fit) + passed = ( + errors.size > 0 + and mae <= self.maximum_validation_mae_rad + and p95 <= self.maximum_validation_p95_rad + and coupling_drift <= self.maximum_coupling_drift_rad + and self.fit.ip_coupling["r_squared"] + >= self.minimum_ip_coupling_r_squared + and self.fit.max_monotonic_correction_rad + <= self.maximum_monotonic_correction_rad + and self.fit.max_hysteresis_rad <= self.maximum_hysteresis_rad + ) + final_payload = create_final_payload( + serial_number=self.serial_number, + fit=self.fit, + validation_errors_rad=self.validation_errors, + passed=passed, + ) + validation_payload = { + "records": self.validation_records, + "summary": { + "passed": passed, + "mae_rad": mae, + "p95_rad": p95, + "maximum_non_target_drift_rad": coupling_drift, + "coupling_anomaly": ( + coupling_drift > self.maximum_coupling_drift_rad + ), + "ip_coupling_r_squared": self.fit.ip_coupling["r_squared"], + "maximum_monotonic_correction_rad": ( + self.fit.max_monotonic_correction_rad + ), + "maximum_hysteresis_rad": self.fit.max_hysteresis_rad, + }, + } + atomic_write_json(self.validation_path, validation_payload) + atomic_write_json(self.final_path, final_payload) + self.state = STATE_COMPLETE + self.state_reason = ( + "calibration_passed" if passed else "calibration_completed_quality_failed" + ) + self._write_checkpoint() + self._write_manifest( + { + "completed_at_utc": datetime.now(timezone.utc).isoformat(), + "quality": validation_payload["summary"], + "final_result": str(self.final_path), + } + ) + self.get_logger().info( + f"Calibration complete: passed={passed}; result={self.final_path}" + ) + + def _write_checkpoint(self) -> None: + payload = { + "state": self.state, + "reason": self.state_reason, + "updated_at_utc": datetime.now(timezone.utc).isoformat(), + "scan_records": len( + [ + record + for record in self.records + if record.get("kind", "sample") == "sample" + ] + ), + "active_task": self.active_task, + "remaining_tasks": len(self.task_queue), + } + atomic_write_json(self.checkpoint_path, payload) + + def _validate_existing_session(self) -> None: + if not self.manifest_path.exists(): + return + try: + existing = json.loads( + self.manifest_path.read_text(encoding="utf-8") + ) + except (OSError, json.JSONDecodeError) as error: + raise ValueError("existing session manifest is invalid") from error + if not isinstance(existing, dict): + raise ValueError("existing session manifest must be an object") + expected = { + "serial_number": self.serial_number, + "tag_config": self.tag_config, + "code_sha256": self._code_sha256(), + } + for key, value in expected.items(): + if existing.get(key) != value: + raise ValueError( + f"existing session {key} does not match; start a new session" + ) + sdk = existing.get("sdk", {}) + if sdk.get("baseline_command_u8") != list(BASELINE_COMMAND): + raise ValueError( + "existing session baseline does not match; start a new session" + ) + if int(sdk.get("calibration_speed_u8", -1)) != self.calibration_speed: + raise ValueError( + "existing session calibration speed does not match; " + "start a new session" + ) + capture = existing.get("capture", {}) + if capture.get("scan_mode", "point") != self.scan_mode: + raise ValueError( + "existing session scan_mode does not match; start a new session" + ) + if ( + capture.get("continuous_motion_mode", "paced") + != self.continuous_motion_mode + ): + raise ValueError( + "existing session continuous_motion_mode does not match; " + "start a new session" + ) + if int(capture.get("repetitions", -1)) != self.repetitions: + raise ValueError( + "existing session repetitions do not match; start a new session" + ) + if int(capture.get("command_step", -1)) != self.command_step: + raise ValueError( + "existing session command_step does not match; start a new session" + ) + + def _write_manifest(self, updates: dict[str, Any] | None = None) -> None: + existing: dict[str, Any] = {} + if self.manifest_path.exists(): + try: + value = json.loads(self.manifest_path.read_text(encoding="utf-8")) + if isinstance(value, dict): + existing = value + except (OSError, json.JSONDecodeError): + pass + payload = { + "schema_version": 1, + "session_kind": "g20_thumb_front_apriltag", + "created_at_utc": existing.get( + "created_at_utc", datetime.now(timezone.utc).isoformat() + ), + "serial_number": self.serial_number, + "code_sha256": self._code_sha256(), + "tag_config": self.tag_config, + "camera": { + "serial_number": self.camera_serial_number, + "camera_info_topic": self.camera_info_topic, + "image_topic": self.image_topic, + "depth_used_for_angle": False, + "angle_estimation_mode": self.angle_estimation_mode, + "debug_image_enabled": self.publish_debug_image, + "intrinsics": self.camera_intrinsics, + }, + "rosbag_path": self.rosbag_path, + "sdk": { + "command_topic": self.command_topic, + "state_topic": self.state_topic, + "baseline_command_u8": list(BASELINE_COMMAND), + "calibration_speed_u8": self.calibration_speed, + "commands_enabled": self.commands_enabled, + "latest_info": self.latest_hand_info, + }, + "capture": { + "scan_mode": self.scan_mode, + "continuous_motion_mode": self.continuous_motion_mode, + "repetitions": self.repetitions, + "command_step": self.command_step, + "auto_start_tip": self.auto_start_tip, + "stable_frames": self.stable_frames, + "capture_frames": self.capture_frames, + "minimum_settle_seconds": self.minimum_settle_seconds, + "maximum_stable_spread_rad": self.maximum_stable_spread_rad, + "pose_outlier_threshold_rad": self.pose_outlier_threshold_rad, + "minimum_pose_inlier_rate": self.minimum_pose_inlier_rate, + "maximum_state_image_skew_ms": ( + self.maximum_state_image_skew_ns / 1_000_000.0 + ), + "continuous_endpoint_tolerance_u8": ( + self.continuous_endpoint_tolerance_u8 + ), + "continuous_endpoint_hold_seconds": ( + self.continuous_endpoint_hold_seconds + ), + "continuous_minimum_valid_frames": ( + self.continuous_minimum_valid_frames + ), + "continuous_minimum_bins": self.continuous_minimum_bins, + "continuous_maximum_bin_gap": ( + self.continuous_maximum_bin_gap + ), + "continuous_segment_minimum_seconds": ( + self.continuous_segment_minimum_seconds + ), + "continuous_segment_timeout_seconds": ( + self.continuous_segment_timeout_seconds + ), + "continuous_prepare_timeout_seconds": ( + self.continuous_prepare_timeout_seconds + ), + }, + } + existing.update(payload) + payload = existing + if updates: + payload.update(updates) + atomic_write_json(self.manifest_path, payload) + + @staticmethod + def _code_sha256() -> str: + digest = hashlib.sha256() + source_dir = Path(__file__).resolve().parent + for name in ("acquisition.py", "core.py", "node.py", "storage.py"): + path = source_dir / name + digest.update(name.encode("utf-8")) + digest.update(path.read_bytes()) + return digest.hexdigest() + + def _publish_status(self) -> None: + active = self.active_task or {} + completed_scan = len( + [ + record + for record in self.records + if record.get("kind", "sample") == "sample" + ] + ) + if self.scan_mode == "continuous": + completed_directions = sum( + self._continuous_direction_complete(phase, direction) + for phase in (PHASE_ROOT, PHASE_TIP) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + scan_progress = completed_directions / 4.0 + else: + total_scan = len( + scan_targets(self.repetitions, self.command_step) + ) * 2 + scan_progress = completed_scan / total_scan + payload = { + "state": self.state, + "reason": self.state_reason, + "commands_enabled": self.commands_enabled, + "calibration_speed_u8": self.calibration_speed, + "angle_estimation_mode": self.angle_estimation_mode, + "scan_mode": self.scan_mode, + "continuous_motion_mode": self.continuous_motion_mode, + "command_step": self.command_step, + "auto_start_tip": self.auto_start_tip, + "active_phase": active.get("phase"), + "active_command_u8": active.get("command_u8"), + "active_direction": active.get("direction"), + "active_cycle": active.get("cycle"), + "collector_state": self.collector.state, + "collector_reason": self.collector.reason, + "stable_frames_seen": self.collector.stable_frames_seen, + "capture_frames_seen": self.collector.capture_frames_seen, + "sweep_collector_state": self.sweep_collector.state, + "sweep_collector_reason": self.sweep_collector.reason, + "sweep_valid_frames_seen": ( + self.sweep_collector.valid_frames_seen + ), + "sweep_state_span_u8": self.sweep_collector.state_span_u8, + "sweep_segment_index": active.get("segment_index"), + "sweep_segment_count": len( + active.get("segment_targets_u8", []) + ), + "scan_progress": scan_progress, + "scan_records": completed_scan, + "session_dir": str(self.session_dir), + } + message = String() + message.data = json.dumps(payload, ensure_ascii=False) + self.status_publisher.publish(message) + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node: G20ThumbCalibrationNode | None = None + try: + node = G20ThumbCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py new file mode 100644 index 0000000..6b575df --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py new file mode 100644 index 0000000..421903c --- /dev/null +++ b/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), + ] + ) diff --git a/src/g20_thumb_apriltag_calibration/package.xml b/src/g20_thumb_apriltag_calibration/package.xml new file mode 100644 index 0000000..08eb9fd --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/package.xml @@ -0,0 +1,35 @@ + + + + g20_thumb_apriltag_calibration + 0.1.0 + Front RealSense AprilTag calibration for the left G20 thumb. + lxp + MIT + + ament_index_python + apriltag_msgs + apriltag_ros + cv_bridge + image_proc + launch + launch_ros + linker_hand_ros2_sdk + rclcpp_components + rclpy + realsense2_camera + rosbag2 + sensor_msgs + std_msgs + std_srvs + tf2_msgs + python3-numpy + python3-opencv + python3-scipy + + python3-pytest + + + ament_python + + diff --git a/src/g20_thumb_apriltag_calibration/pyproject.toml b/src/g20_thumb_apriltag_calibration/pyproject.toml new file mode 100644 index 0000000..09977b5 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" diff --git a/src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration b/src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration @@ -0,0 +1 @@ + diff --git a/src/g20_thumb_apriltag_calibration/setup.cfg b/src/g20_thumb_apriltag_calibration/setup.cfg new file mode 100644 index 0000000..bae3e00 --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py new file mode 100644 index 0000000..b6d0fce --- /dev/null +++ b/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", + ], + }, +) diff --git a/src/g20_thumb_apriltag_calibration/test/test_acquisition.py b/src/g20_thumb_apriltag_calibration/test/test_acquisition.py new file mode 100644 index 0000000..e9aaea8 --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py new file mode 100644 index 0000000..b101054 --- /dev/null +++ b/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" diff --git a/src/g20_thumb_apriltag_calibration/test/test_core.py b/src/g20_thumb_apriltag_calibration/test/test_core.py new file mode 100644 index 0000000..adf543b --- /dev/null +++ b/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 + ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_storage.py b/src/g20_thumb_apriltag_calibration/test/test_storage.py new file mode 100644 index 0000000..896523a --- /dev/null +++ b/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"}]