拇指侧摆零位标定

.gitignore

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

Readme_ZH.md

@@ -1,5 +1,8 @@
 # 1. LinkerFFG手套
 
+> 当前FFG多手势标定、G20/O6 profile映射及实机操作请优先参考：
+> [FFG多手势标定映射与遥操操作说明](docs/FFG多手势标定映射与遥操操作说明.md)。
+
 ## 1.1 产品介绍
 本产品的具体介绍参考，内含标定示例说明
 附件1、Linker FFG(FFG01)产品说明手册
@@ -416,4 +419,4 @@ if self.calibrationoriginal is not None and self.calibrationfistpose is not None
 
 改写成如上图的示例，即可启用右机械手的校准，启动后就会让机械手按照映射的角度固定在当前角度
 
-当左右两手都达到期望的对指位置后，就可以恢复原状，按正常顺序使用遥操系统
+当左右两手都达到期望的对指位置后，就可以恢复原状，按正常顺序使用遥操系统

docs/FFG多手势标定映射与遥操技术实现.md

@@ -0,0 +1,985 @@
+# FFG多手势标定映射与遥操技术实现
+
+## 1. 文档定位
+
+本文面向 `linkerforce_v2` 的开发、联调和维护人员，说明新版FFG手套映射遥操链路的
+软件架构、标定拟合方法、实时映射算法、ROS 2接口、profile约束和安全门控。
+
+实机标定与启动步骤见
+[《FFG多手势标定映射与遥操操作说明》](./FFG多手势标定映射与遥操操作说明.md)。
+本文不重复完整操作流程，而是回答以下实现问题：
+
+- 21维FFG数据如何变成模型无关的手部语义；
+- 张手、桌面、钩拳、握拳四个锚点如何解耦根部和末端屈伸；
+- G20和O6如何共用手套语义、同时保持各自的机械执行标尺；
+- 捏合与握持为什么不会把整只手锁定到离散模板；
+- `cmd_u8`、`actuation_target`、`q_nominal`三类目标有什么区别；
+- profile如何生成、校验、配对和追踪；
+- 节点在什么条件下允许或撤销实机控制。
+
+当前实现基于：
+
+- profile schema：`schema_version=1`；
+- 手套侧：单只左手FFG，21维输入；
+- 机械手侧：左手G20和左手O6；
+- 映射模式：`factorized_paired_v2`；
+- 机械手profile策略：`paired_continuous_v1`；
+- 仿真策略：`semantic_urdf_v1`；
+- 标定等级：`provisional`，没有真实关节角GT。
+
+## 2. 代码组织
+
+核心实现位于
+[`linkerforce_v2`](../src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/)：
+
+| 文件 | 职责 |
+|---|---|
+| `constants.py` | FFG关节名、手部语义名、静态/动态手势集合和默认参数 |
+| `calibrate_glove.py` | 订阅FFG原始话题，交互采集完整标定和快速佩戴检查 |
+| `calibrate_robot.py` | 从GUI命令、快照和SDK状态生成G20/O6实机profile |
+| `calibration.py` | 鲁棒统计、FFG特征拟合、机械手通道权重和分段曲线拟合 |
+| `profiles.py` | profile加载、严格校验、规范化哈希和原子保存 |
+| `mapping.py` | 人手语义提取、连续配对映射、捏合/握持修正和命令滤波 |
+| `node.py` | ROS 2实时节点、话题、服务、定时循环和安全门控 |
+| `safety.py` | 与ROS无关的超时撤权判定 |
+| `simulation.py` | 按关节名重排仿真目标并执行限位检查 |
+| `quality.py` | 静态、捏合、动态轨迹的离线回放质量检查 |
+| `verify_robot_profile.py` | 低速回放机械手标定姿势并生成独立人工复核报告 |
+| `session_manifest.py` | 生成provisional数采清单并绑定profile、URDF和设备信息 |
+
+ROS 2入口在
+[`setup.py`](../src/linkerhand_retarget/setup.py)，双手机型启动文件为
+[`ffg_dual_g20_o6.launch.py`](../src/linkerhand_retarget/launch/ffg_dual_g20_o6.launch.py)，
+可提交的机械手种子profile位于
+[`resource/linkerforce_v2/profiles`](../src/linkerhand_retarget/resource/linkerforce_v2/profiles/)。
+
+## 3. 总体架构
+
+```text
+                         ┌──────────────────────────────┐
+FFG串口 / ROS JointState│  21维左手套原始弧度 raw[21] │
+                         └──────────────┬───────────────┘
+                                        │ 可选逐维Kalman
+                                        ▼
+                         ┌──────────────────────────────┐
+                         │ HandIntentExtractor          │
+                         │ 21维 → 22维0～1人手语义     │
+                         └──────────────┬───────────────┘
+                                        │ 同一hand_intent
+                    ┌───────────────────┴───────────────────┐
+                    ▼                                       ▼
+          ┌───────────────────┐                   ┌───────────────────┐
+          │ G20 RobotMapper   │                   │ O6 RobotMapper    │
+          │ 16个主动执行语义  │                   │ 6个主动执行语义   │
+          └─────────┬─────────┘                   └─────────┬─────────┘
+                    │                                       │
+          ┌─────────┴─────────┐                   ┌─────────┴─────────┐
+          ▼                   ▼                   ▼                   ▼
+   20维cmd_u8          16维q_nominal       6维cmd_u8           6维q_nominal
+   G20实机电机空间      G20仿真弧度目标      O6实机电机空间       O6仿真弧度目标
+```
+
+架构的关键是分成三层：
+
+1. **传感器层**：FFG原始值和佩戴差异由手套profile吸收；
+2. **解剖语义层**：`hand_intent`只描述人的手部动作，不依赖G20或O6；
+3. **执行器层**：每个机械手profile独立定义语义到本机电机命令和URDF目标的映射。
+
+因此，换机械手通常不需要改手套语义提取器；换手套或操作者也不需要改G20/O6的
+通道定义，只需重新建立对应profile并在运行时配对。
+
+## 4. 数据契约
+
+### 4.1 FFG 21维原始数据
+
+原始输入使用 `sensor_msgs/msg/JointState`。名称和规范顺序为：
+
+```text
+thumb_0 ... thumb_4
+index_0 ... index_3
+middle_0 ... middle_3
+ring_0 ... ring_3
+pinky_0 ... pinky_3
+```
+
+串口解析器将设备角度转换为弧度。实时节点只接受长度为21、全部有限的数据。
+
+- 串口模式直接使用上述顺序；
+- topic模式允许输入名称顺序不同，但要求名称集合完整且唯一，节点按规范顺序重排；
+- 标定工具要求输入消息已经使用规范顺序；
+- 不支持右手FFG，也不会因右手套缺失而退出。
+
+普通四指每指4个原始量：第0维主要用于侧摆，第1～3维共同参与根部和末端屈伸拟合。
+拇指5个原始量由不同组合共同拟合旋转、外展、对掌、根部屈伸和末端屈伸。
+
+### 4.2 人手语义
+
+`hand_intent`共22维，所有值限制在 `[0, 1]`：
+
+| 类别 | 名称 |
+|---|---|
+| 拇指 | `thumb_rotate`、`thumb_abduction`、`thumb_opposition`、`thumb_root`、`thumb_tip` |
+| 四指屈伸 | 每指的 `<finger>_root`、`<finger>_tip` |
+| 四指侧摆 | 每指的 `<finger>_splay` |
+| 捏合证据 | `pinch_index`、`pinch_middle`、`pinch_ring`、`pinch_pinky` |
+| 整体握持 | `power_grasp` |
+
+这里的0和1是由个人手套标定定义的语义端点，不是机械手角度，也不代表统一的物理角度。
+
+### 4.3 G20命令空间
+
+G20输出完整20维 `cmd_u8`：
+
+| 下标 | 通道 |
+|---:|---|
+| 0 | `thumb_cmc_pitch` |
+| 1～4 | `index/middle/ring/pinky_mcp_pitch` |
+| 5 | `thumb_cmc_roll` |
+| 6～9 | `index/middle/ring/pinky_mcp_roll` |
+| 10 | `thumb_cmc_yaw` |
+| 11～14 | `reserved_11`～`reserved_14`，固定为255 |
+| 15 | `thumb_mcp` |
+| 16～19 | `index/middle/ring/pinky_pip` |
+
+其中16个通道是主动映射通道，4个保留通道不参与映射。新版种子profile中
+`thumb_cmc_yaw`使用完整的 `[0, 255]` 命令范围，不再继承旧版的80下限。
+
+### 4.4 O6命令空间
+
+O6输出6维 `cmd_u8`：
+
+```text
+thumb_cmc_pitch
+thumb_cmc_yaw
+index_mcp_pitch
+middle_mcp_pitch
+ring_mcp_pitch
+pinky_mcp_pitch
+```
+
+O6没有独立的四指PIP和侧摆执行通道，因此每个普通手指的单一屈伸通道由
+`root`和`tip`语义融合得到。
+
+### 4.5 三种输出标尺
+
+| 输出 | 范围/单位 | 含义 |
+|---|---|---|
+| `actuation_target` | `[0,1]` | 当前型号各主动通道的归一化语义激活量 |
+| `cmd_u8_preview` / 实机命令 | `[0,255]` | 设备电机命令空间，包含机械耦合和本机标定 |
+| `joint_target_nominal` | rad | 根据语义激活量和URDF名义端点生成的仿真目标 |
+
+`state_u8`是SDK返回的设备状态，仍属于设备空间。它既不是编码器关节角，也不能作为
+`q_nominal`或真实物理关节角的GT。
+
+## 5. FFG手套profile的生成
+
+### 5.1 鲁棒采样统计
+
+每次采集保留：
+
+```text
+sample_count
+median[21]
+mad[21]
+raw_frames[N][21]
+```
+
+对第 `j` 维：
+
+```text
+median_j = median(raw[:, j])
+MAD_j    = median(abs(raw[:, j] - median_j))
+```
+
+每个静态姿势和动态轨迹还保存3次独立重复的上述统计。`approved_for_runtime=true`
+要求：
+
+- 11个静态姿势全部存在；
+- 7个动态轨迹全部存在；
+- 每项恰好3次重复；
+- 每次至少50帧；
+- 汇总帧数等于3次重复的帧数之和。
+
+因此，CLI虽然允许修改静态 `--repeats`，但不是3次时生成的profile只能用于预览。
+
+### 5.2 基础语义特征拟合
+
+每个语义特征定义一组原始下标和带标签的标定姿势。以某个特征为例：
+
+1. 从语义标签为0的姿势求原始端点 `low`；
+2. 从语义标签为1的姿势求原始端点 `high`；
+3. 将各标定姿势归一化为：
+
+```text
+n_j = clip((raw[index_j] - low_j) / (high_j - low_j), 0, 1)
+```
+
+4. 用最小二乘拟合各原始维度对语义标签的贡献；
+5. 将负权重截为0，再归一化为权重和1；
+6. 运行时计算：
+
+```text
+feature = clip(sum(weight_j * n_j), 0, 1)
+```
+
+无有效跨度的维度不参与归一化。若拟合后所有权重都接近0，则回退为等权。
+
+普通四指的 `root`和`tip`故意使用同一组3个屈伸传感器，但使用不同姿势标签：
+
+| 姿势 | root目标 | tip目标 |
+|---|---:|---:|
+| 张手/并拢 | 0 | 0 |
+| 桌面 | 1 | 0 |
+| 钩拳 | 0 | 1 |
+| 握拳 | 1 | 1 |
+
+这一设计先得到两个可能仍有耦合的初始特征，再由下一步二维标定面解耦。
+
+### 5.3 根部—末端双线性解耦
+
+对每个普通手指，在初始 `(root_feature, tip_feature)` 平面中取得四个锚点：
+
+```text
+p00 = 张手
+p10 = 桌面
+p01 = 钩拳
+p11 = 握拳
+```
+
+建立双线性标定面：
+
+```text
+p(u, v) = p00
+        + (p10 - p00) * u
+        + (p01 - p00) * v
+        + (p11 - p10 - p01 + p00) * u * v
+```
+
+其中 `u`是解耦后的根部屈伸，`v`是解耦后的末端屈伸。运行时先用线性最小二乘得到
+初值，再执行最多5次Newton迭代反解 `(u, v)`，最后限制到 `[0,1]`。
+
+只有标定四边形在四角的Jacobian行列式符号一致，且最小绝对值不小于 `1e-3` 时才启用
+该解码器。退化或发生折叠的标定面不会用于反解，此时保留基础特征结果。
+
+### 5.4 动态屈伸对侧摆的串扰补偿
+
+每个普通手指的独立屈伸往返轨迹假设该手指侧摆应基本不变。对每次重复：
+
+```text
+x = 0.5 * (root + tip)
+y = splay
+x, y分别减去各自中位数
+coefficient = dot(x, y) / (dot(x, x) + ridge)
+```
+
+其中 `ridge = 1e-3 * max(dot(x,x), 1e-6)`。
+
+以下情况拒绝学习该次轨迹：
+
+- 少于10帧、长度错误或存在非有限值；
+- 屈伸变化范围小于0.05；
+- 侧摆几乎没有变化，无法估计；
+- 补偿后残差方差仍大于原方差的80%；
+- 三次重复的有效系数方向互相矛盾。
+
+最终系数取各有效重复的中位数并限制到 `[-1,1]`，运行时执行：
+
+```text
+splay_corrected = clip(
+    splay - coefficient * 0.5 * root
+          - coefficient * 0.5 * tip,
+    0,
+    1
+)
+```
+
+补偿只发生在人手语义层，不直接学习或修改任何G20/O6电机系数。
+
+### 5.5 捏合证据
+
+每种捏合只使用拇指5维和目标手指4维。标定时保存：
+
+```text
+center = 目标捏合姿势中位数
+scale  = max(abs(center - open), 6 * pinch_pose_MAD, 0.02)
+```
+
+并计算张手到捏合中心的归一化距离 `open_distance`。运行时：
+
+```text
+distance = RMS((raw_selected - center) / scale)
+pinch_strength = clip(1 - distance / open_distance, 0, 1)
+```
+
+这4个值是候选证据，不直接等于4个离散状态；最终是否施加捏合修正还要经过竞争门控。
+
+### 5.6 快速佩戴检查
+
+快速检查重新采集张手、握拳和食指捏合。每个姿势计算：
+
+```text
+normalized_error =
+    RMS((observed_median - reference_median) / max(6 * MAD, 0.05))
+```
+
+默认要求每个误差不大于4.0。凭据保存当前手套profile的规范化SHA-256、检查时间、
+阈值、各姿势误差和通过状态。
+
+实时节点只在加载profile时检查凭据：
+
+- `kind=ffg_wear_check`；
+- `passed=true`；
+- 绑定哈希等于当前手套profile哈希；
+- 凭据年龄在配置范围内，默认12小时。
+
+节点不会在长时间运行期间周期性重新读取凭据或重新计算年龄。需要跨时段运行时，应按
+作业流程主动重启节点并重新执行佩戴检查。
+
+## 6. 机械手profile的生成
+
+### 6.1 捕获数据
+
+`hand_pose_capture`同时监听：
+
+- GUI连续命令 `/<model>/cb_left_hand_control_cmd`；
+- SDK状态 `/<model>/cb_left_hand_state`；
+- GUI保存快照 `/<model>/calibration_pose_snapshot`。
+
+消息名称允许任意顺序，但必须与seed中的 `command_names`集合完全一致；保存前统一重排为
+profile顺序。每个姿势保存：
+
+```text
+cmd_u8
+command_names
+state_u8
+state_names
+status = exact | approximate | unsupported
+confirmed
+captured_at
+```
+
+每次人工确认后立即原子写入checkpoint。恢复时会核对seed哈希、型号、输出路径、
+SN、固件、CAN、操作者、命令名和姿势列表；身份不一致时拒绝续标。若旧checkpoint中
+某个命令超出新的安全范围，只删除该姿势并要求重拍。
+
+最终 `approved_for_control=true` 同时要求：
+
+- 用户在最后明确批准；
+- SN、CAN和操作者非空；
+- 所有必需姿势均已确认；
+- 命令与状态名称完整；
+- 所有命令位于profile安全范围。
+
+`unsupported`表示该姿势不参与对应运行时约束，但该姿势记录本身仍需人工确认并保存。
+
+### 6.2 多源执行通道权重
+
+一个机械手主动通道可以融合多个人手语义。对seed中列出的 `fit_sources`，使用
+张手、桌面、钩拳和握拳的实机命令拟合。
+
+先以张手和握拳命令归一化该通道：
+
+```text
+y_pose = (cmd_pose - cmd_open) / (cmd_fist - cmd_open)
+```
+
+设计矩阵来自各姿势的规范语义目标，然后执行最小二乘；负权重截为0并归一化。
+
+- G20大部分主动通道只有一个语义源；
+- O6普通手指通道同时使用对应的 `root`和`tip`，权重由实机捕获结果拟合；
+- 若张手和握拳命令跨度退化，回退为等权。
+
+### 6.3 分段曲线与单调约束
+
+profile生成时，根据通道语义激活量和各标定姿势的 `cmd_u8`产生曲线点。
+
+- `piecewise`：同一激活量的命令取中位数，然后按激活量排序；
+- `monotonic_piecewise`：在上述基础上使用相邻违例合并算法执行等距单调回归；
+- 曲线至少需要两个不同的激活量；
+- 命令点必须位于 `[0,255]`和该通道 `command_bounds`内。
+
+该profile曲线是一条可独立验证和追踪的型号级基线，也是在运行时手套锚点退化时的
+单通道回退曲线。
+
+## 7. 运行时分解式配对映射
+
+### 7.1 配对曲线构造
+
+`RobotMapper`同时收到手套profile和机械手profile时，不直接使用抽象规范姿势坐标，
+而是：
+
+1. 用当前手套profile的静态姿势中位数重新计算真实 `hand_intent`；
+2. 找出手套和机械手共有且未标为 `unsupported` 的姿势；
+3. 对每个机械手主动通道，选择真正定义该解剖通道的姿势；
+4. 以当前手套语义激活量为横轴、当前实机profile命令为纵轴重建分段曲线；
+5. 对单调通道再次执行单调回归。
+
+姿势选择规则为：
+
+| 通道 | 使用的基础姿势 |
+|---|---|
+| 普通四指屈伸 | 张手、并拢、桌面、钩拳、握拳中双方共有的姿势 |
+| G20普通四指侧摆 | 并拢、张手 |
+| 拇指基础通道 | 最大外展、张手、横跨掌心 |
+
+捏合姿势不进入普通通道曲线，握拳也不直接进入拇指基础曲线；它们分别由局部残差分支
+处理。这样，某个捏合捕获中的非目标手指残留命令不会污染普通手指曲线。
+
+如果某个通道的实际手套锚点退化为少于两个不同激活量，该通道使用机械手profile中
+已经校验的曲线；其他通道仍可保持配对曲线。
+
+完成构造后，`mapping_mode`为 `factorized_paired_v2`。
+
+### 7.2 基础通道映射
+
+对第 `k` 个主动通道，其来源权重为 `w_ki`，当前人手语义为 `h_i`：
+
+```text
+a_k = clip(sum(w_ki * h_i) / sum(w_ki), 0, 1)
+```
+
+`a_k`组成 `actuation_target`。基础电机命令由该通道配对曲线分段线性插值得到：
+
+```text
+cmd_base[index_k] = piecewise_linear(a_k, paired_points_k)
+```
+
+完整命令向量先以张手命令初始化，主动通道逐个覆盖；未映射的保留通道之后强制写回
+固定值。
+
+### 7.3 竞争式局部捏合修正
+
+#### 7.3.1 标定自适应阈值
+
+映射器先对手套profile中的所有静态姿势计算4种捏合分数。对每个真实捏合姿势记录：
+
+- 目标分数；
+- 目标分数相对其他3种分数的领先量。
+
+对所有非捏合姿势记录：
+
+- 最大误触分数；
+- 第一名相对第二名的误触领先量。
+
+满激活阈值取4个目标姿势中的最弱值，起始阈值位于最大负样本与满激活阈值之间的20%：
+
+```text
+score_onset  = negative_score  + 0.2 * (score_full  - negative_score)
+margin_onset = negative_margin + 0.2 * (margin_full - negative_margin)
+```
+
+若当前手套profile无法在正负样本间形成有效分数或领先量间隔，所有捏合门均保持0。
+
+#### 7.3.2 单赢家连续门控
+
+运行时仅选择当前分数最高的候选，并计算：
+
+```text
+score_gate  = smoothstep((top_score - score_onset) / score_span)
+margin_gate = smoothstep((top_score - second_score - margin_onset) / margin_span)
+pinch_gate  = score_gate * margin_gate
+```
+
+`smoothstep(x)=x²(3-2x)`，输入先限制到 `[0,1]`。其他3种捏合门为0。
+
+因此：
+
+- 证据不足时保持普通连续映射；
+- 两种捏合证据接近时，领先量门控将修正降到0；
+- 不存在确认帧数、进入/退出滞回或历史姿势锁存；
+- 捏合切换只依赖当前帧，且权重连续变化。
+
+#### 7.3.3 局部残差
+
+对每种捏合，在该手套捏合中位数处先计算基础命令，再与机械手目标捏合命令做差：
+
+```text
+residual = robot_pinch_target - base_command_at_glove_pinch
+```
+
+运行时只把 `pinch_gate * residual`加到：
+
+- 所有拇指主动通道；
+- 当前目标手指的主动通道。
+
+其他3根手指不参与该分支。G20的保留通道也不参与。
+
+### 7.4 握持时的拇指协调
+
+`power_grasp`是8个普通四指 `root/tip`语义的平均值。握持分数进一步要求拇指主动折叠：
+
+```text
+grasp_score = min(
+    power_grasp,
+    thumb_opposition,
+    thumb_root,
+    thumb_tip
+)
+```
+
+满分取手套握拳姿势，负样本取其他静态姿势的最高分，门控同样使用从负样本到握拳分数
+20%处开始的 `smoothstep`。
+
+握持残差是机械手握拳目标与握拳处基础命令的差，但只施加到拇指主动通道。四指仍由
+各自连续屈伸曲线决定，普通拇指动作也不会仅因四指弯曲而被强制成握拳拇指。
+
+### 7.5 安全范围与保留通道
+
+局部修正完成后依次执行：
+
+1. 写回保留通道固定值；
+2. 按每通道 `command_bounds`裁剪；
+3. 执行可选命令滤波；
+4. 再次裁剪并再次写回保留通道；
+5. 最终四舍五入为整数命令。
+
+`raw_command`保留滤波前浮点目标，当前ROS节点不发布该字段；`cmd_u8_preview`发布滤波后
+并取整的最终目标。
+
+## 8. 滤波与实时执行
+
+### 8.1 输入Kalman
+
+输入滤波是21个互相独立的一维Kalman滤波器，共享参数：
+
+```text
+P_pred = P + process_variance
+K      = P_pred / (P_pred + measurement_variance)
+x      = x + K * (z - x)
+P      = (1 - K) * P_pred
+```
+
+首帧、时间倒退或帧间隔超过 `input_filter_reset_gap` 时直接重置到当前测量，避免断流后
+从旧状态缓慢追赶。默认关闭。
+
+### 8.2 输出命令滤波
+
+`CommandFilter`支持：
+
+| 模式 | 行为 |
+|---|---|
+| `passthrough` | 直接使用本帧目标，仅应用deadband |
+| `ema` | `step=clip(alpha*(target-last), ±max_step)` |
+| `acceleration_limited` | 同时限制速度、帧间加速度，并根据剩余距离提前制动 |
+
+默认参数匹配旧版左手G20的有效执行路径：
+
+```text
+input_filter_enabled=false
+command_filter_mode=passthrough
+command_filter_ema_alpha=1.0
+command_filter_max_step_u8=255
+command_filter_deadband_u8=0
+```
+
+实时节点没有单独暴露 `command_filter_max_acceleration_u8_per_frame2` 参数；
+`acceleration_limited`模式下它使用与 `command_filter_max_step_u8`相同的值。
+
+### 8.3 30 Hz最新帧策略
+
+实时节点的处理定时器默认30 Hz：
+
+1. 串口模式从线程安全快照取得最新序列号、数据和接收时刻；
+2. 只有出现新FFG序列时，才发布/更新raw、filtered、intent和frame metadata；
+3. 每个定时周期都用最近一次有效intent重新计算两个型号目标；
+4. 预览始终发布，只有已使能型号才发布到SDK命令话题；
+5. 硬件命令QoS为 `RELIABLE + KEEP_LAST(depth=1)`。
+
+固定控制心跳不会排队重放旧手套帧。FFG停止更新时，节点可在超时窗口内短暂复用最后
+intent，随后watchdog撤销使能。
+
+使能某型号时，命令滤波器会重置到该型号最新有效SDK状态，而不是张手或上一次内部
+目标，从而降低重新使能的第一帧跳变。
+
+## 9. 独立仿真目标
+
+仿真目标不从 `cmd_u8`反解。对主动通道激活量 `a_k`：
+
+```text
+q_nominal_k = clip(
+    q_open_k + a_k * (q_closed_k - q_open_k),
+    q_lower_k,
+    q_upper_k
+)
+```
+
+其输入是基础解剖通道激活量，不使用电机命令曲线，也不直接使用捏合或握持的电机残差。
+因此实机姿势捕获中的机械耦合、偶然残留值和保留通道不会污染仿真弧度目标。
+
+仿真消费者必须按 `JointState.name`建立映射。`simulation.py`提供：
+
+- `build_name_mapping()`：检查空名、重名、缺名和多余名称；
+- `reorder_named_target()`：按仿真模型顺序重排，检查有限值并应用仿真限位。
+
+名称合同不满足时抛出 `JointNameMismatch`，不得按裸下标猜测。
+
+profile中的 `urdf_sha256`用于追踪生成名义端点时对应的URDF版本，但实时映射节点本身
+不读取或重新计算URDF文件哈希；数采manifest工具会执行文件哈希核对。
+
+## 10. ROS 2实时节点
+
+### 10.1 输入与输出话题
+
+| 话题 | 类型 | 维度 | 发布条件 |
+|---|---|---:|---|
+| `/ffg/left/raw_joint_state` | `JointState` | 21 | 串口模式收到新帧；topic模式直接使用上游话题 |
+| `/ffg/left/filtered_joint_state` | `JointState` | 21 | 有有效手套profile和新帧 |
+| `/retarget/left/hand_intent` | `JointState` | 22 | 有有效手套profile和新帧 |
+| `/retarget/left/frame_meta` | `String(JSON)` | - | 每个新映射手套帧 |
+| `/retarget/g20/left/actuation_target` | `JointState` | 16 | G20 mapper有效 |
+| `/retarget/o6/left/actuation_target` | `JointState` | 6 | O6 mapper有效 |
+| `/retarget/g20/left/joint_target_nominal` | `JointState` | 16 | G20 mapper有效 |
+| `/retarget/o6/left/joint_target_nominal` | `JointState` | 6 | O6 mapper有效 |
+| `/retarget/g20/left/cmd_u8_preview` | `JointState` | 20 | G20 mapper有效 |
+| `/retarget/o6/left/cmd_u8_preview` | `JointState` | 6 | O6 mapper有效 |
+| `/g20/cb_left_hand_control_cmd` | `JointState` | 20 | G20已使能 |
+| `/o6/cb_left_hand_control_cmd` | `JointState` | 6 | O6已使能 |
+| `/ffg_dual_retarget/status` | `String(JSON)` | - | 1 Hz |
+
+节点订阅：
+
+| 话题 | 说明 |
+|---|---|
+| `raw_input_topic` | `input_mode=topic`时的FFG输入，默认 `/ffg/left/raw_joint_state` |
+| `/g20/cb_left_hand_state` | G20驱动状态心跳 |
+| `/o6/cb_left_hand_state` | O6驱动状态心跳 |
+
+topic输入模式不会再次向raw话题发布收到的消息，避免默认同名话题形成反馈。
+
+驱动状态的名称必须已经按profile `command_names`规范顺序排列；这里与FFG topic输入不同，
+不会对驱动状态按集合重排。
+
+### 10.2 帧元数据
+
+`frame_meta`包含：
+
+```json
+{
+  "timestamp_ns": 0,
+  "sequence": 0,
+  "calibration": "provisional",
+  "q_gt": null
+}
+```
+
+一个新手套帧产生的filtered、intent、frame_meta及该次定时周期的型号目标共用ROS时间戳。
+定时器复用旧intent时，型号目标使用新的当前时间戳，但不会重复发布intent和frame_meta。
+
+### 10.3 状态诊断
+
+1 Hz状态JSON包含：
+
+- 当前输入模式、FFG帧序号和数据年龄；
+- G20/O6分别是否使能；
+- glove、G20、O6的批准状态和SHA-256；
+- `mapping_mode`、`simulation_mapping_mode`；
+- 输入和命令滤波配置；
+- 当前捏合/握持局部分支权重 `anchor_weights`；
+- wear-check有效性；
+- 驱动状态年龄；
+- profile和URDF哈希；
+- profile加载错误、最近故障和p95调度延迟。
+
+`latency_p95_ms`以本机接收手套数据的单调时钟为起点，表示接收至映射调度的延迟，
+不是基于设备硬件时间戳的端到端链路延迟。
+
+### 10.4 服务与状态转换
+
+| 服务 | 类型 | 作用 |
+|---|---|---|
+| `~/enable_g20` | `SetBool` | 单独申请/撤销G20实机控制 |
+| `~/enable_o6` | `SetBool` | 单独申请/撤销O6实机控制 |
+| `~/enable_all` | `SetBool` | 原子检查两台后同时使能，或同时撤销 |
+| `~/emergency_stop` | `Trigger` | 立即撤销两个型号的命令发布权限 |
+
+```text
+                         显式SetBool(true)且全部检查通过
+              ┌──────────────────────────────────────────┐
+              │                                          ▼
+         PREVIEW / DISABLED                         ENABLED(model)
+              ▲                                          │
+              └──────────────────────────────────────────┘
+                SetBool(false)、超时、映射异常或软件急停
+```
+
+撤销使能的含义是停止向SDK命令话题发布新命令，不会主动发送张手、零位或其他安全姿势。
+驱动/固件将保持最后命令相关行为；物理急停仍应由系统级安全链路负责。
+
+## 11. 实机使能门控
+
+某个型号从PREVIEW进入ENABLED前依次检查：
+
+1. 手套profile已加载且 `approved_for_runtime=true`；
+2. wear-check已通过启动时校验；
+3. 机械手profile已加载且 `approved_for_control=true`；
+4. 启动参数中的期望SN非空，并与profile SN完全一致；
+5. profile CAN接口与启动配置一致；
+6. mapper为 `factorized_paired_v2`；
+7. FFG最近一帧未超过 `glove_timeout`，默认0.35秒；
+8. 对应SDK状态名称、长度、数值和范围有效，且未超过 `driver_timeout`，默认1秒。
+
+运行时身份门控比较SN和CAN接口，不比较profile中的固件版本。固件兼容性目前依赖操作
+流程和数采manifest的可选校验；若固件变更会改变电机响应，应重新标定机械手profile。
+
+`enable_all`先检查G20和O6两者，任意一个失败都不会使能任何一个。单型号服务互相独立。
+
+## 12. Watchdog与故障策略
+
+watchdog周期为50 ms：
+
+| 故障 | 动作 |
+|---|---|
+| 任意型号已使能且FFG超时 | 同时撤销G20和O6 |
+| 某型号SDK状态无效或超时 | 只撤销该型号，另一型号保持 |
+| 映射计算出现数值/形状错误 | 同时撤销G20和O6 |
+| 软件急停 | 同时撤销G20和O6 |
+| profile加载失败 | 启动时降级；不创建对应mapper或只保留raw |
+
+故障恢复不会自动重新使能。排除原因后必须再次调用对应使能服务。
+
+节点启动和profile加载采用fail-closed策略：
+
+- 无手套profile：只发布原始FFG；
+- 手套profile有效但未获运行批准：允许完整预览，拒绝实机；
+- seed机械手profile `approved_for_control=false`：允许预览，拒绝实机；
+- 单个型号profile无效：另一个有效型号仍可生成目标和独立使能。
+
+## 13. 主要ROS参数
+
+### 13.1 FFG输入
+
+| 参数 | 默认值 | 说明 |
+|---|---|---|
+| `input_mode` | `serial` | `serial`或`topic` |
+| `raw_input_topic` | `/ffg/left/raw_joint_state` | topic模式输入 |
+| `serial_port` | 空 | 指定串口；为空时可自动扫描 |
+| `baudrate` | `0` | 大于0时优先尝试该波特率 |
+| `baudrates` | `[2000000,460800,1000000,921600]` | 探测候选 |
+| `auto_scan` | `true` | 指定端口失败或为空时扫描 |
+| `serial_debug` | `false` | 串口调试日志 |
+
+### 13.2 Profile与身份
+
+| 参数 | 默认值 | 说明 |
+|---|---|---|
+| `glove_profile` | 空 | FFG完整标定profile |
+| `wear_check` | 空 | 快速佩戴检查凭据 |
+| `wear_check_max_age_hours` | `12.0` | 启动加载时允许的最大年龄 |
+| `g20_profile` / `o6_profile` | 空 | 单机机械手profile |
+| `g20_serial_number` / `o6_serial_number` | 空 | 运行期望SN |
+| `g20_can_interface` | `can0` | G20身份核对 |
+| `o6_can_interface` | `can1` | O6身份核对 |
+
+### 13.3 时序和滤波
+
+| 参数 | 默认值 | 说明 |
+|---|---:|---|
+| `publish_rate` | `30.0` | 固定映射/命令心跳Hz |
+| `glove_timeout` | `0.35` | FFG超时秒数 |
+| `driver_timeout` | `1.0` | SDK状态超时秒数 |
+| `input_filter_enabled` | `false` | 是否启用逐维Kalman |
+| `input_filter_process_variance` | `1e-5` | Kalman过程噪声 |
+| `input_filter_measurement_variance` | `5e-4` | Kalman测量噪声 |
+| `input_filter_reset_gap` | `0.35` | 断流重置阈值 |
+| `command_filter_mode` | `passthrough` | 输出滤波模式 |
+| `command_filter_ema_alpha` | `1.0` | EMA/限加速度目标增益 |
+| `command_filter_max_step_u8` | `255.0` | 每帧最大速度尺度 |
+| `command_filter_deadband_u8` | `0.0` | 小于该差值时保持上一目标 |
+
+启动文件还负责创建两个SDK节点，并设置启动速度、力矩、状态轮询和G20控制期间延迟状态
+读取等驱动参数；这些不是 `ffg_dual_retarget`自身参数。
+
+## 14. Profile校验与可追踪性
+
+### 14.1 严格加载
+
+`profiles.py`在构造mapper之前检查：
+
+- schema、profile类型、型号和左手侧；
+- 固定的FFG关节名或机械手命令名；
+- 所有数组长度、有限值和范围；
+- 特征权重非负且和为1；
+- 分段曲线激活量、命令范围和单调性；
+- 主动通道与保留通道完整覆盖命令向量；
+- 仿真名称顺序、端点、限位和URDF哈希格式；
+- 已批准profile的设备身份、人工确认、状态和名称完整性。
+
+profile错误不会被静默修正为另一种型号或旧映射策略。
+
+### 14.2 规范化哈希
+
+profile哈希不是原文件字节哈希，而是：
+
+1. 排除加载器添加的 `_profile_path`和 `_profile_sha256`；
+2. JSON key排序；
+3. 使用紧凑分隔符和UTF-8；
+4. 计算SHA-256。
+
+因此仅缩进或JSON键顺序变化不会改变profile身份，持久字段变化会改变哈希。
+
+保存使用同目录临时文件加原子替换，避免中途退出留下半个JSON。
+
+### 14.3 相关运行文件
+
+| 文件 | 技术作用 |
+|---|---|
+| glove profile | 原始帧、鲁棒统计、特征参数和捏合锚点 |
+| wear-check | 绑定glove profile哈希的短期佩戴凭据 |
+| robot profile | 设备身份、姿势、通道曲线、安全范围和仿真端点 |
+| checkpoint | 绑定seed和设备元数据的可恢复捕获进度 |
+| verification | 绑定robot profile哈希的独立人工复核结果 |
+| session manifest | 绑定profile、wear-check、URDF、设备和rosbag话题 |
+
+`session_manifest.py`当前要求G20和O6都已批准，并按 `can0/can1`核对；它适用于标准双手
+型号数采拓扑，不是任意单型号或任意CAN配置的通用manifest生成器。
+
+## 15. 离线质量检查
+
+`retarget_profile_check`不启动ROS、不连接机械手，直接回放profile中的原始帧和姿势。
+
+### 15.1 静态复现
+
+对每个共有姿势，只比较该姿势真正定义的相关通道：
+
+- 捏合：拇指和目标手指；
+- 拇指姿势：拇指通道；
+- 并拢：侧摆通道；
+- 桌面/钩拳：普通四指屈伸通道；
+- 握拳：除普通侧摆外的通道。
+
+任一相关通道最大误差大于5个u8单位，记为hard failure。
+
+### 15.2 捏合混淆
+
+四个捏合中位数必须：
+
+- 竞争winner等于目标手指；
+- 目标门控不小于0.95。
+
+否则记为hard failure。
+
+### 15.3 动态连续性和局部性
+
+每组动态重复记录：
+
+- 目标通道跨度；
+- 非目标通道跨度；
+- 原始浮点命令帧间步长p95和最大值；
+- 取整后整帧不变比例；
+- 应用profile执行滤波后的同类指标。
+
+普通手指屈伸轨迹中，若非目标通道跨度中位数大于
+`max(15, 0.2 * target_span)`，生成warning。四指开合轨迹中若任一屈伸语义范围中位数
+大于0.5，也生成warning。
+
+动态步长目前只报告统计量，没有统一hard-failure阈值；应结合采样率、动作速度和设备
+允许步长分析。
+
+离线通过只证明profile内部复现和分解逻辑满足这些判据，不证明实机物理角度精度。
+
+## 16. 实机姿势复核实现
+
+`hand_pose_verify`加载已批准机械手profile后：
+
+1. 查询命令话题是否已有其他发布者，有则拒绝开始；
+2. 要求显式输入安全确认；
+3. 从最新SDK状态而不是上一次目标开始；
+4. 将目标分成每通道步长不超过 `max_step_u8` 的线性序列；
+5. 默认30 Hz发送，运动中周期检查新竞争发布者；
+6. 稳定后读取命名状态并计算设备空间绝对误差；
+7. 保存人工通过/失败、备注、目标、状态和误差摘要。
+
+默认 `max_step_u8=4`，CLI硬限制不超过8。复核报告明确记录
+`state_is_angle_ground_truth=false`，并且不修改原机械手profile。
+
+## 17. 扩展和维护约束
+
+### 17.1 增加新的机械手型号
+
+至少需要：
+
+1. 在 `MODEL_COMMAND_LENGTHS`登记型号和命令长度；
+2. 定义唯一、稳定的 `command_names`；
+3. 创建seed profile，包括姿势、安全范围、主动通道、保留通道和仿真端点；
+4. 明确每个执行通道的解剖语义源；
+5. 扩展profile校验器的必需姿势集合；
+6. 扩展节点的话题、身份参数、状态和服务；
+7. 增加静态复现、局部性、限位和名称合同测试。
+
+不要通过复制G20的裸下标映射来接入新型号；名称、主动通道和保留通道必须显式定义。
+
+### 17.2 增加新的手套语义
+
+需要同步更新：
+
+- `BASE_INTENT_NAMES`或派生语义列表；
+- 标定姿势标签和原始下标；
+- glove profile生成及严格校验；
+- `HandIntentExtractor.extract()`输出顺序；
+- 使用该语义的机械手seed和测试；
+- rosbag/下游消费者的数据合同。
+
+修改名称或顺序会影响profile兼容性，应升级schema而不是让旧profile静默通过。
+
+### 17.3 修改手势或阈值
+
+捏合和握持阈值由当前手套profile自动推导。优先修复标定数据或距离定义，不要增加隐藏
+的全局常量绕过竞争判据。若确需改变门控公式，应同时更新：
+
+- 正/负样本定义；
+- 连续性和混淆测试；
+- 离线质量报告；
+- `mapping_mode`或schema版本，以便数据可追踪。
+
+### 17.4 线程与实时性
+
+- 串口读取在线程中更新带锁快照；
+- ROS节点定时器只消费最新快照，不阻塞等待串口；
+- 运行时没有无界命令队列；
+- 标定和复核CLI可使用后台executor线程，因为它们包含交互式终端等待；
+- 映射主要是小向量NumPy运算，不包含在线优化或模型推理。
+
+## 18. 测试与验收建议
+
+核心单元/集成测试集中在
+[`test_linkerforce_v2.py`](../src/linkerhand_retarget/test/test_linkerforce_v2.py)，覆盖：
+
+- 鲁棒统计和profile完整性；
+- 根部/末端解耦和曲线内部无平台；
+- 捏合局部性、连续切换和无历史锁存；
+- 普通手指对拇指动作的独立性；
+- 动态侧摆串扰补偿；
+- 仿真目标与电机残差隔离；
+- 滤波步长、加速度、重置和默认直通行为；
+- profile身份、checkpoint、安全范围和命名合同；
+- 重使能重基准和超时撤权；
+- G20/O6输出长度、名称和限位。
+
+修改核心算法后至少执行：
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+python3 -m pytest -q \
+  src/linkerhand_retarget/test/test_linkerforce_v2.py
+```
+
+完成profile标定后再分别执行G20和O6离线质量回放。涉及ROS接口、SDK命名或launch参数的
+修改，还应在PREVIEW状态检查实际话题长度、名称、频率和status JSON，再进入低速实机
+验收。
+
+## 19. 已知边界
+
+- 当前只支持左手FFG到左手G20/O6；
+- `provisional`不提供真实物理关节角精度声明；
+- `cmd_u8`和SDK `state_u8`不能转换成可靠的真实关节弧度；
+- 仿真目标只代表语义—URDF名义映射，不是实机测量；
+- 捏合竞争无时间滞回，连续性依赖当前帧证据质量和可选输入滤波；
+- wear-check只在节点加载时验证，不在长时间运行中自动过期撤权；
+- 运行时身份门控不核对固件版本；
+- 软件急停只撤销发布权限，不替代硬件急停或独立安全控制器；
+- 默认双型号launch会同时创建两个SDK驱动；单型号系统可直接启动所需驱动和
+  `ffg_dual_retarget`节点。
+
+这些边界应保留在数据报告、实验结论和对外精度声明中。

docs/FFG多手势标定映射与遥操操作说明.md

@@ -0,0 +1,731 @@
+# FFG多手势标定映射与遥操操作说明
+
+## 1. 文档目的
+
+本文档说明当前 `linkerforce_v2` 无Marker方案的工作原理、标定流程和实机操作方法。
+当前主要使用场景是一只左手FFG控制左手G20，也支持在配置对应profile后同时生成O6目标。
+
+当前方案属于 `provisional` 阶段：
+
+- 可以验证手套语义、机械手通道、方向、动作范围和连续性；
+- 可以用于演示和临时数采；
+- 不能把G20/O6的 `0～255` 电机命令当作真实关节角；
+- 没有Marker、编码器或独立角度传感器时，不能给出实机与仿真的真实角度误差。
+
+旧入口 `handretarget` 仍然保留；本文档只描述新入口 `ffg_dual_retarget`。
+
+## 2. 当前映射架构
+
+```text
+FFG左手套21维原始数据
+        ↓
+hand_intent：模型无关的人手语义（0～1）
+        ↓
+        ├─→ G20 actuation_target
+        │      ├─→ G20单机profile → 20维cmd_u8 → G20实机
+        │      └─→ G20名义URDF范围 → q_nominal → 仿真
+        │
+        └─→ O6 actuation_target
+               ├─→ O6单机profile → 6维cmd_u8 → O6实机
+               └─→ O6名义URDF范围 → q_nominal → 仿真
+```
+
+实机命令和仿真目标是两条独立标尺：
+
+- `cmd_u8`：设备电机空间命令，范围为0～255；
+- `q_nominal`：根据URDF名义限位生成的仿真弧度目标；
+- `state_u8`：SDK返回的设备状态，只用于运行诊断，不是真实关节角GT。
+
+仿真不应直接把 `cmd_u8` 当作真实角度。需要接近实机外观时，可以使用同一
+`actuation_target`，再通过实测角度标定完善仿真标尺。
+
+## 3. 多手势标定解决什么问题
+
+### 3.1 FFG静态姿势
+
+完整手套标定采集11个静态姿势：
+
+1. 五指自然张开、自然分开；
+2. 五指伸直并拢；
+3. 桌面手势：四指根部弯曲、末端伸直；
+4. 钩拳：四指根部伸直、末端弯曲；
+5. 自然握拳；
+6. 拇指最大外展；
+7. 拇指横跨掌心；
+8. 拇指—食指捏合；
+9. 拇指—中指捏合；
+10. 拇指—无名指捏合；
+11. 拇指—小指捏合。
+
+每个静态姿势默认采集2秒、重复3次，每次至少50帧。profile保留全部原始帧、
+每次中位数、MAD和有效帧数，而不是只保存一个平均值。
+
+### 3.2 FFG动态轨迹
+
+完整标定还采集7组短时往返轨迹：
+
+- 食指独立弯曲往返；
+- 中指独立弯曲往返；
+- 无名指独立弯曲往返；
+- 小指独立弯曲往返；
+- 拇指弯曲往返；
+- 拇指对掌往返；
+- 四指开合往返。
+
+动态轨迹主要用于发现和补偿同一手指屈伸对侧摆语义的传感器串扰，并检查非目标
+手指是否跟随运动。它们不是额外的离散手势模板。
+
+### 3.3 连续映射原则
+
+当前运行时不会把整只手吸附到“最相似的标定手势”：
+
+- 每根普通手指只读取自身的根部、末端和侧摆语义；
+- 张手、桌面、钩拳和握拳构成四指根部—末端标定面，连续解耦传感器串扰；
+- 普通屈伸映射保持连续，不在曲线内部加入停止平台；
+- 四种捏合分别进行竞争判断；
+- 捏合只局部修正拇指和目标手指，不替换整只手命令；
+- 捏合证据不明确时，连续退回普通逐关节映射；
+- 握拳只增加必要的拇指协调，不把相似动作强制变成握拳模板。
+
+### 3.4 当前实时执行策略
+
+当前默认执行节奏与旧版左手G20的有效路径一致：
+
+```text
+publish_rate=30Hz
+input_filter_enabled=false
+command_filter_mode=passthrough
+command_filter_ema_alpha=1.0
+command_filter_max_step_u8=255
+command_filter_deadband_u8=0
+repeat_position_commands=true
+```
+
+也就是直接使用最新手套帧，将连续电机目标交给G20固件插值，并在每个30Hz控制心跳
+重复发送最新目标。待发送队列深度为1，来不及发送时只保留最新目标，不重放旧命令。
+
+Kalman和EMA仍然可以显式启用，但不要同时启用两层滤波。两层滤波叠加后再进行整数
+取整，容易表现为小幅运动停顿、累计后跳变。
+
+## 4. profile与运行文件
+
+| 文件 | 内容 | 是否提交Git |
+|---|---|---|
+| `glove_<ID>_left_<operator>.json` | 个人佩戴下的FFG完整标定 | 否 |
+| `*.wear_check.json` | 绑定手套profile哈希的快速佩戴检查 | 否 |
+| `hand_G20_left_<SN>_provisional.json` | 指定G20实机的姿势命令profile | 否 |
+| `*.checkpoint.json` | 实机姿势标定中断续标检查点 | 否 |
+| `*.verification.json` | 实机姿势人工复核报告 | 否 |
+| `g20_seed_profile.json` | G20 GUI安全初值和标定结构 | 是 |
+| `o6_seed_profile.json` | O6 GUI安全初值和标定结构 | 是 |
+
+根目录 `profiles/` 已加入 `.gitignore`。可复现的种子profile位于：
+
+```text
+src/linkerhand_retarget/resource/linkerforce_v2/profiles/
+```
+
+手套profile与操作者、手套和佩戴方式相关；机械手profile与型号、序列号、固件和CAN
+接口相关。换手套、换操作者、明显改变佩戴位置时，应重新完整标定FFG。换机械手本体
+或固件导致电机响应变化时，应重新标定机械手profile。
+
+## 5. 构建与环境准备
+
+在每个新终端中都要加载ROS和工作空间：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+```
+
+源码修改后重新构建：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+
+colcon build --symlink-install --packages-select \
+  linker_hand_ros2_sdk gui_control linkerhand_retarget
+
+source install/setup.bash
+```
+
+如果出现 `Package 'linkerhand_retarget' not found`，通常是当前终端没有执行上述两个
+`source`，或者源码修改后还没有构建。
+
+## 6. 完整标定FFG左手套
+
+### 6.1 启动FFG原始数据发布
+
+终端A启动只读FFG节点。只连接左手套即可，不要求右手套存在：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p serial_port:=/dev/ttyUSB0 \
+  -p baudrate:=2000000 \
+  -p auto_scan:=true
+```
+
+确认原始话题有数据：
+
+```bash
+ros2 topic hz /ffg/left/raw_joint_state
+```
+
+如果标定提示“0个有效帧”，不要继续重复按Enter。先确认：
+
+- 终端A仍在运行；
+- 日志显示左手FFG已连接；
+- `/dev/ttyUSB0`没有被另一个FFG进程占用；
+- `/ffg/left/raw_joint_state`有稳定数据。
+
+### 6.2 执行完整手套标定
+
+终端B执行：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --glove-id FFG_LEFT_SN \
+  --operator lxp \
+  --firmware 2.1.4 \
+  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json
+```
+
+静态姿势的正确操作：
+
+1. 先摆好终端提示的固定姿势；
+2. 姿势稳定后按Enter；
+3. 按Enter后继续保持不动约2秒；
+4. 终端显示保存帧数后再放松；
+5. 同一姿势按相同方法重复3次。
+
+动态往返轨迹的正确操作：
+
+1. 先回到该动作的自然起始位置；
+2. 按Enter后立即开始连续、缓慢地往返运动；
+3. 在默认3秒采集窗口内完成若干次完整往返；
+4. 非目标手指尽量保持稳定；
+5. 不要先弯好后全程静止，否则采不到动态关系。
+
+标定成功应显示：
+
+```text
+approved_for_runtime=True
+sha256=<手套profile哈希>
+```
+
+不要使用 `--skip-dynamic` 生成正式运行profile。该参数只适合调试。
+
+## 7. 快速佩戴检查
+
+每次正式实机启动前，对当前准备使用的手套profile执行快速检查：
+
+```bash
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --quick-check /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --quick-output /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json
+```
+
+依次检查张手、握拳和食指捏合。每个姿势也是“先摆好，再按Enter，然后保持2秒”。
+
+快速检查凭据：
+
+- 默认12小时有效；
+- 必须显示 `passed=true`；
+- 必须与启动时使用的手套profile SHA-256完全一致；
+- 切换v4、v5等手套profile时，必须同时切换到对应的wear-check文件。
+
+快速检查失败时，先重新调整手套佩戴位置并重试。如果多次失败，说明当前佩戴与完整
+标定差异过大，应重新完整标定，不要通过增大阈值静默放行实机。
+
+## 8. 标定G20实机姿势
+
+### 8.1 安全要求
+
+- 标定时使用低速、低力矩；
+- 配备软件急停，并保证机械手周围无障碍物；
+- 不得使用旧手套映射把机械手带到标定姿势；
+- 不得在带电状态强行手掰；
+- 使用GUI逐通道调整，并确认通道方向正确。
+
+### 8.2 启动G20 SDK
+
+终端A：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
+  -p hand_type:=left \
+  -p hand_joint:=G20 \
+  -p is_touch:=false \
+  -p can:=can0 \
+  -p modbus:=None \
+  -p topic_prefix:=/g20 \
+  -p startup_speed:=30 \
+  -p startup_torque:=80 \
+  -p move_on_startup:=false \
+  -p state_poll_rate:=10.0
+```
+
+### 8.3 启动G20标定GUI
+
+终端B：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run gui_control gui_control --ros-args \
+  -r __node:=g20_calibration_gui \
+  -p hand_type:=left \
+  -p hand_joint:=G20 \
+  -p topic_prefix:=/g20
+```
+
+### 8.4 捕获11个G20姿势
+
+终端C：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linkerhand_retarget hand_pose_capture -- \
+  --model G20 \
+  --seed /home/lxp/projects/linkerhand_retarget_ros2/install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/g20_seed_profile.json \
+  --serial-number G20_LEFT_001 \
+  --operator lxp \
+  --firmware unknown \
+  --can-interface can0 \
+  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json
+```
+
+每个姿势的操作：
+
+1. 用GUI低速调整机械手；
+2. 目视确认目标手指、通道方向和最终姿势；
+3. 等待实机稳定；
+4. 点击GUI“保存当前标定姿势”；
+5. CLI收到快照后选择姿势状态。
+
+状态含义：
+
+- `exact`：机械手能够准确实现该姿势；
+- `approximate`：受机构自由度限制，只能实现最佳近似；
+- `unsupported`：该型号不能可靠实现，不用于对应姿势约束；
+- 输入 `r`：放弃刚才的快照，重新调整和保存。
+
+CLI每完成一个姿势都会立即写入 `*.checkpoint.json`。程序中断后，使用完全相同的
+命令会自动恢复并跳过已保存姿势，不需要从头开始。
+
+只有确实要放弃原进度时才增加：
+
+```text
+--fresh
+```
+
+全部姿势完成后，只有输入 `y` 批准，最终profile才会包含：
+
+```text
+approved_for_control=True
+```
+
+O6操作相同，但使用 `--model O6`、O6 seed、`can1`和 `/o6` 命名空间。O6自由度较少，
+桌面、钩拳及部分捏合通常应标为 `approximate`。
+
+## 9. 复核机械手profile
+
+姿势复核可以避免手工把JSON中的20维命令复制到GUI。
+
+复核前：
+
+- 停止 `ffg_dual_retarget`；
+- 停止GUI，避免命令话题存在其他发布者；
+- 只保留低速、低力矩的G20 SDK。
+
+执行：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20
+```
+
+只复核单个姿势：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20 \
+  --pose pinch_index
+```
+
+按照提示输入 `VERIFY`、`MOVE`，再选择：
+
+- `p`：目视通过；
+- `f`：目视未通过；
+- `r`：重放；
+- `s`：跳过。
+
+工具会低速平滑过渡，并保存独立的 `*.verification.json`，不会修改原始机械手profile。
+
+## 10. 离线检查映射质量
+
+不连接实机即可回放profile中的静态与动态数据：
+
+```bash
+ros2 run linkerhand_retarget retarget_profile_check -- \
+  --glove /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --robot /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
+  --model G20 \
+  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/g20_mapping_quality.json
+```
+
+重点查看：
+
+- `passed`和`hard_failures`；
+- 静态有效通道复现误差；
+- 四种捏合的winner和gate；
+- 四指动态轨迹的非目标通道跨度；
+- 小指、侧摆等动作是否有明显串扰；
+- 帧间命令变化是否存在异常突跳。
+
+离线检查通过不等于实机角度准确，只表示profile内部逻辑一致。
+
+## 11. 启动G20正式遥操
+
+正式启动前，停止旧SDK、标定GUI、姿势捕获工具和占用FFG串口的只读节点。每种节点
+只保留一个实例。
+
+### 11.1 终端A：启动G20驱动
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
+  -p hand_type:=left \
+  -p hand_joint:=G20 \
+  -p is_touch:=false \
+  -p can:=can0 \
+  -p modbus:=None \
+  -p topic_prefix:=/g20 \
+  -p startup_speed:=255 \
+  -p startup_torque:=80 \
+  -p move_on_startup:=false \
+  -p state_poll_rate:=10.0 \
+  -p velocity_poll_rate:=10.0 \
+  -p defer_state_reads_while_commanding:=true \
+  -p repeat_position_commands:=true
+```
+
+`startup_speed`控制电机最大运动速度；`startup_torque`控制最大输出力矩。提高力矩不会
+解决映射卡顿。建议先使用80，在确有负载需要并完成安全评估后再提高。
+
+确认状态话题已有发布者：
+
+```bash
+ros2 topic info /g20/cb_left_hand_state
+```
+
+应至少显示：
+
+```text
+Publisher count: 1
+```
+
+### 11.2 终端B：启动FFG映射节点
+
+以下示例使用当前v4手套profile：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p serial_port:=/dev/ttyUSB0 \
+  -p baudrate:=2000000 \
+  -p auto_scan:=true \
+  -p publish_rate:=30.0 \
+  -p input_filter_enabled:=false \
+  -p command_filter_mode:=passthrough \
+  -p command_filter_ema_alpha:=1.0 \
+  -p command_filter_max_step_u8:=255.0 \
+  -p command_filter_deadband_u8:=0.0 \
+  -p glove_profile:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp_v4.json \
+  -p wear_check:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp_v4.wear_check.json \
+  -p g20_profile:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
+  -p g20_serial_number:=G20_LEFT_001 \
+  -p g20_can_interface:=can0
+```
+
+正常日志应包含：
+
+```text
+FFG profile已加载
+三姿势快速佩戴检查有效
+G20 profile已加载(可申请实机使能)
+G20映射=factorized_paired_v2
+执行滤波=passthrough alpha=1.0, max_step=255.0
+左手FFG已连接
+```
+
+节点启动后默认处于PREVIEW，不会立即控制实机。
+
+### 11.3 PREVIEW检查
+
+在使能前观察：
+
+```text
+/ffg/left/raw_joint_state
+/ffg/left/filtered_joint_state
+/retarget/left/hand_intent
+/retarget/g20/left/actuation_target
+/retarget/g20/left/joint_target_nominal
+/retarget/g20/left/cmd_u8_preview
+```
+
+检查要求：
+
+- 张手、半握、握拳过程中命令连续；
+- 弯曲一根手指时，主要变化的是对应手指通道；
+- 食指捏合主要影响拇指与食指；
+- 不应锁定在某个历史捏合模板；
+- 不应出现明显越限或突跳。
+
+### 11.4 使能G20
+
+终端C：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 service call /ffg_dual_retarget/enable_g20 \
+  std_srvs/srv/SetBool "{data: true}"
+```
+
+成功返回：
+
+```text
+success=True
+message='G20已使能'
+```
+
+停用G20：
+
+```bash
+ros2 service call /ffg_dual_retarget/enable_g20 \
+  std_srvs/srv/SetBool "{data: false}"
+```
+
+软件急停：
+
+```bash
+ros2 service call /ffg_dual_retarget/emergency_stop \
+  std_srvs/srv/Trigger "{}"
+```
+
+急停、FFG断开、驱动状态超时或profile错误后，都需要排除问题并重新显式使能。
+
+## 12. ROS话题说明
+
+| 话题 | 说明 |
+|---|---|
+| `/ffg/left/raw_joint_state` | 21维FFG原始数据 |
+| `/ffg/left/filtered_joint_state` | 实际送入语义提取器的数据；默认与raw相同 |
+| `/retarget/left/hand_intent` | 模型无关的0～1人手语义 |
+| `/retarget/g20/left/actuation_target` | G20归一化目标 |
+| `/retarget/g20/left/joint_target_nominal` | G20仿真名义弧度目标 |
+| `/retarget/g20/left/cmd_u8_preview` | 未使能时也持续发布的20维预览命令 |
+| `/g20/cb_left_hand_control_cmd` | 使能后发送给G20 SDK的20维命令 |
+| `/g20/cb_left_hand_state` | G20 SDK状态心跳 |
+| `/ffg_dual_retarget/status` | profile、使能、超时、滤波和延迟诊断 |
+
+所有向量都带名称。仿真桥和其他消费者必须按 `JointState.name` 匹配，不得依赖裸下标。
+
+## 13. 安全与自动停用
+
+- 默认PREVIEW，必须通过服务显式使能；
+- FFG超过0.35秒没有新帧：撤销全部实机使能；
+- 对应驱动状态超过1秒未更新：只撤销该型号；
+- profile缺失、未批准、SN/CAN不匹配：拒绝实机使能；
+- wear-check缺失、失败、过期或哈希不匹配：拒绝实机使能；
+- 所有命令检查长度、名称、有限值和0～255范围；
+- G20四个保留通道保持安全固定值；
+- 带电机械手不得强行手掰。
+
+## 14. 常见问题排查
+
+### 14.1 `Package 'linkerhand_retarget' not found`
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
+```
+
+如果仍然找不到，重新执行第5节的构建命令。
+
+### 14.2 手套标定只有0个有效帧
+
+原因通常是没有单独启动FFG原始数据发布节点。检查：
+
+```bash
+ros2 topic info /ffg/left/raw_joint_state
+ros2 topic hz /ffg/left/raw_joint_state
+```
+
+### 14.3 快速佩戴检查失败
+
+- 确认使用的是正确版本profile；
+- 调整手套位置、腕带和手指传感器；
+- 每个姿势先摆好再按Enter；
+- 按Enter后保持不动2秒；
+- 多次失败则重新完整标定。
+
+### 14.4 `三姿势快速佩戴检查缺失、失败或过期`
+
+重新对启动时使用的同一个手套profile执行第7节命令。不能拿v5的wear-check启动v4。
+
+### 14.5 `G20驱动状态无效或已超时`
+
+先执行：
+
+```bash
+ros2 topic info /g20/cb_left_hand_state
+```
+
+如果 `Publisher count: 0`，说明G20 SDK未启动或没有使用 `/g20` 命名空间。按第11.1节
+启动驱动。如果有发布者，再检查：
+
+```bash
+ros2 topic echo /g20/cb_left_hand_state --once
+```
+
+状态必须是20维、名称与G20 profile一致、数值有限且位于0～255。
+
+### 14.6 服务一直显示 `waiting for service`
+
+检查：
+
+```bash
+ros2 node list
+ros2 service list | grep ffg_dual_retarget
+```
+
+常见原因是 `ffg_dual_retarget`没有启动、当前终端未source，或者服务名称中多写了反斜杠。
+
+### 14.7 服务成功但机械手不动
+
+检查命令话题：
+
+```bash
+ros2 topic info /g20/cb_left_hand_control_cmd
+ros2 topic hz /g20/cb_left_hand_control_cmd
+```
+
+使能后应同时存在发布者和订阅者，并接近30Hz。还要检查SDK终端是否报告CAN错误。
+
+### 14.8 机械手运动卡顿
+
+确认运行参数：
+
+```bash
+ros2 param get /ffg_dual_retarget input_filter_enabled
+ros2 param get /ffg_dual_retarget command_filter_mode
+ros2 param get /linker_hand_sdk repeat_position_commands
+```
+
+当前推荐结果：
+
+```text
+False
+passthrough
+True
+```
+
+同时检查：
+
+- 只运行一个G20 SDK和一个映射节点；
+- 命令话题稳定接近30Hz；
+- CAN状态查询在遥操期间已延后；
+- 不要用提高力矩解决卡顿；
+- 如果只有某一根手指异常，运行离线profile质量检查，重点看该手指动态轨迹。
+
+### 14.9 某根手指张手不到位或发生串指
+
+依次比较：
+
+```text
+raw_joint_state
+→ hand_intent
+→ cmd_u8_preview
+→ cb_left_hand_state
+```
+
+- raw异常：佩戴或FFG采集问题；
+- hand_intent异常：手套标定/语义解耦问题；
+- intent正确但preview异常：映射/profile问题；
+- preview正确但实机异常：机械手profile、驱动、固件或机构问题。
+
+不要直接通过修改某个写死系数掩盖问题。
+
+### 14.10 实机姿势标定中断
+
+使用完全相同的 `hand_pose_capture` 命令重新启动，会自动读取检查点并跳过已保存姿势。
+不要增加 `--fresh`，除非明确要删除当前标定进度并从头开始。
+
+## 15. 当前精度边界与后续优化
+
+当前多手势方案可以继续优化：
+
+- 重采质量较差的小指、侧摆或拇指动态轨迹；
+- 改善21维传感器到人体语义的连续解耦；
+- 完善拇指对掌和四种捏合的局部连续映射；
+- 为每台机械手建立方向相关、非线性的电机命令曲线；
+- 记录输入、映射、发布、CAN和状态时间戳，量化延迟与丢帧。
+
+要得到可量化的真实角度精度，仍需增加Marker、编码器或独立角度传感器，建立：
+
+```text
+hand_intent
+    → 实机真实关节角q_target
+    → 单机关节角—cmd_u8标定
+```
+
+在此之前，验收结论只能是动作语义、通道、连续性和外观接近程度，不能声明实机与仿真
+达到某个真实关节角误差。
+
+## 16. 正式运行前检查清单
+
+- [ ] ROS与工作空间已source；
+- [ ] 当前只有一个FFG读取进程；
+- [ ] 当前只有一个G20 SDK，使用`can0`和`/g20`；
+- [ ] FFG profile显示`approved_for_runtime=True`；
+- [ ] wear-check通过、未过期且哈希匹配；
+- [ ] G20 profile显示`approved_for_control=True`；
+- [ ] profile中的SN和CAN与启动参数一致；
+- [ ] `/g20/cb_left_hand_state`有有效发布者；
+- [ ] PREVIEW下逐指、握拳和四种捏合动作正确；
+- [ ] 默认实时参数为关闭输入滤波、直通命令、30Hz重复目标；
+- [ ] 周围安全、急停可用；
+- [ ] 最后才调用`enable_g20`。

src/g20_thumb_apriltag_calibration/README.md

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

src/g20_thumb_apriltag_calibration/config/calibration.yaml

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

src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml

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

src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml

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

src/g20_thumb_apriltag_calibration/config/front_tags.yaml

@@ -0,0 +1,30 @@
+/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    # Live calibration needs the newest frame, not lossless delivery of stale
+    # frames. BEST_EFFORT prevents a slow full-resolution detection callback
+    # from back-pressuring image_proc's reliable image publisher.
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.01
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3]
+      frames: [tag_t0, tag_t3, tag_t4, tag_t5]
+      sizes: [0.010, 0.010, 0.010, 0.010]
+
+g20_thumb_calibration:
+  ros__parameters:
+    tag_roles: [t0, t3, t4, t5]
+    tag_ids: [0, 1, 2, 3]
+    tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
+    tag_sizes_m: [0.010, 0.010, 0.010, 0.010]

src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py

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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py

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

src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py

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

src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py

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

src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py

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

src/g20_thumb_apriltag_calibration/package.xml

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

src/g20_thumb_apriltag_calibration/pyproject.toml

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

src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration

@@ -0,0 +1 @@
+

src/g20_thumb_apriltag_calibration/setup.cfg

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

src/g20_thumb_apriltag_calibration/setup.py

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

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

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

src/g20_thumb_apriltag_calibration/test/test_config.py

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

src/g20_thumb_apriltag_calibration/test/test_core.py

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

src/g20_thumb_apriltag_calibration/test/test_diagnostics.py

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

src/g20_thumb_apriltag_calibration/test/test_pnp.py

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

src/g20_thumb_apriltag_calibration/test/test_storage.py

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

src/g20_thumb_apriltag_calibration/test/test_trajectory.py

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

src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py

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

src/gui_control/gui_control/gui_control.py

@@ -38,7 +38,7 @@ _CANONICAL_COMMAND_NAMES = {
 _CANONICAL_COMMAND_BOUNDS = {
     "G20": [
         *[(0, 255)] * 10,
-        (80, 255),
+        (0, 255),
         *[(255, 255)] * 4,
         *[(0, 255)] * 5,
     ],

src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md

@@ -174,8 +174,8 @@ CLI将GUI保存动作和状态选择共同视为人工确认，并立即写入
 提示当前姿势，不会退出。程序被关闭或异常中断后，使用完全相同的命令
 会校验型号、seed哈希、SN、固件、CAN、操作者和输出路径，并自动跳过已保存姿势。
 若旧检查点中的某些命令超出profile安全范围，CLI会保留其他有效姿势，只移除并
-重拍超限姿势。G20 GUI同时把 `thumb_cmc_yaw` 限制在80～255，并将4个保留通道
-固定为255；不得为了复现超限手势而放宽这些安全范围。
+重拍超限姿势。G20的 `thumb_cmc_yaw` 使用完整0～255命令范围，4个保留通道仍固定
+为255；不得修改保留通道的固定值。
 只有明确希望放弃旧进度时才在原命令末尾增加 `--fresh`；该选项会覆盖旧检查点，
 从第一个姿势重新开始。
 

src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json

@@ -93,7 +93,7 @@
   "command_bounds": [
     [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
     [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
-    [80, 255], [255, 255], [255, 255], [255, 255], [255, 255],
+    [0, 255], [255, 255], [255, 255], [255, 255], [255, 255],
     [0, 255], [0, 255], [0, 255], [0, 255], [0, 255]
   ],
   "reserved_channels": {

src/linkerhand_retarget/test/test_linkerforce_v2.py

@@ -248,6 +248,24 @@ def test_seed_profiles_map_named_bounded_commands(model, length):
         assert result.command[11:15] == (255, 255, 255, 255)
 
 
+def test_g20_thumb_yaw_uses_full_command_range():
+    profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    assert profile["command_bounds"][10] == [0.0, 255.0]
+    yaw_channel = next(
+        channel
+        for channel in profile["channels"]
+        if channel["name"] == "thumb_cmc_yaw"
+    )
+    yaw_channel["points"] = [[0.0, 255.0], [1.0, 0.0]]
+    intent = {name: 0.0 for name in BASE_INTENT_NAMES}
+    intent["thumb_rotate"] = 1.0
+    result = RobotMapper(profile).map(intent)
+    assert result.raw_command[10] == 0.0
+    assert result.command[10] == 0
+
+
 def test_paired_pinch_anchors_only_constrain_thumb_and_target_finger():
     glove_profile = build_glove_profile(
         _glove_captures(),
@@ -429,9 +447,7 @@ def test_thumb_mapping_ignores_four_finger_only_pose_commands():
     altered_profile = copy.deepcopy(robot_profile)
     for pose_name in ("fingers_together", "tabletop", "hook"):
         for command_index in (0, 5, 10, 15):
-            altered_profile["poses"][pose_name]["cmd_u8"][command_index] = (
-                80.0 if command_index == 10 else 0.0
-            )
+            altered_profile["poses"][pose_name]["cmd_u8"][command_index] = 0.0
 
     extractor = HandIntentExtractor(glove_profile)
     reference_mapper = RobotMapper(robot_profile, glove_profile)
@@ -629,9 +645,9 @@ def test_robot_pose_checkpoint_resumes_and_rejects_identity_mismatch(tmp_path):
 def test_robot_capture_rejects_commands_outside_profile_safety_bounds():
     seed = load_robot_profile(PROFILE_DIR / "g20_seed_profile.json", "G20")
     command = list(seed["poses"]["open_spread"]["cmd_u8"])
-    command[10] = 79.0
+    command[10] = -1.0
     assert _command_bound_violations(seed, command) == (
-        "thumb_cmc_yaw=79，允许[80, 255]",
+        "thumb_cmc_yaw=-1，允许[0, 255]",
     )