O30临时标定提交

.gitignore

@@ -50,12 +50,45 @@ Thumbs.db
 
 # Runtime and calibration scratch files
 /logs/
+/MvSdkLog/
 *.tmp
 *.log
+*.bak
+*.orig
+*.rej
+
+# Operator/device-specific calibration artifacts
+# Reproducible seed profiles remain under
+# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
+/profiles/
+/calibration_output/
+/config/*_three_camera_extrinsics.yaml
+*.wear_check.json
+*.checkpoint.json
+*.verification.json
+*_mapping_quality.json
+
+# Device-specific robot descriptions derived from local calibration runs
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf
 
 # ROS bag / MCAP recordings and CAN captures
 rosbag2_*/
+/bags/
+/recordings/
+/captures/
+/sessions/
+/reports/
 *.db3
 *.mcap
 candump-*
-l10_*_state_*/
+*_state_*/
+
+# Local Codex/agent workspace metadata
+/.agents/
+/.codex/

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`。

docs/G20三相机零位标定与URDF修正操作说明.md

@@ -0,0 +1,669 @@
+# G20 三相机零位标定与 URDF 修正操作说明
+
+> 适用工程：`linkerhand_retarget_ros2`  
+> 适用标定包：`g20_thumb_apriltag_calibration`  
+> 文档基线：2026-08-11 当前 V3 轴坐标系逻辑  
+> 适用对象：G20 左手和右手
+
+## 1. 文档目的
+
+本文说明当前代码中 G20 三相机、11 个 AprilTag 的完整标定逻辑及现场操作流程，包含：
+
+- 20 通道 u8 命令到 21 个 URDF 关节动态角度曲线的标定；
+- 拇指 CMC roll/yaw/pitch 的静态 URDF 零位求解；
+- 四指动态曲线继承和静态 CAD 零位保护；
+- 第三轮留出验证、自动重采、暂停和恢复策略；
+- 从原始 CAD URDF 生成修正 URDF；
+- 使用已有 `raw_samples.jsonl` 进行无运动离线重放；
+- 修正 URDF 和配套 JSON 的仿真使用方法。
+
+本文中的右手 `G20_RIGHT_001/20260811_120146` 数值仅用于说明当前算法的实际结果，**不是代码中写死的标定角度，也不是其他机械手的目标值**。
+
+## 2. 标定的最终产物
+
+一次完整标定同时生成两类互相配套的产物：
+
+1. 精简标定 JSON：保存每个关节按命令 `0～255` 索引的 256 点 `angle_rad` 动态曲线，以及 URDF 静态零偏和质量指标。
+2. 修正 URDF：只把通过验证的静态零偏写入原始 CAD URDF 的主动关节 `origin.rpy`。
+
+运行时必须遵守以下规则：
+
+- 修正 URDF 已经包含静态 `urdf_zero_offset_rad`，运行时不能再加一次；
+- 动态关节位置必须使用同一机械手、同一侧、同一次标定 JSON 中的 `angle_rad`；
+- 左手曲线不能用于右手，右手曲线不能用于左手；
+- 不能用已经修正过的 URDF 作为下一次标定的源文件。
+
+## 3. 硬件和坐标配置
+
+### 3.1 三台相机
+
+默认机位和序列号如下：
+
+| 机位 | 默认序列号 | 默认作用 |
+|---|---|---|
+| 正面 `front` | `DB2163742` | 拇指 CMC pitch/roll、MCP/IP、参考指 MCP roll |
+| 侧面 `side` | `DB2163749` | 参考指 MCP pitch、PIP/DIP |
+| 上面 `top` | `DB2163739` | 拇指 CMC yaw |
+
+默认采集参数：
+
+- 分辨率和像素格式由海康节点配置；
+- 帧率：`30 Hz`；
+- 曝光：`5000 us`；
+- 增益：`0 dB`；
+- 自动曝光：关闭；
+- AprilTag 检测降采样：`decimate=1.5`；
+- 启动文件固定使用 `rmw_fastrtps_cpp` 和 64 MB Fast DDS 共享内存配置。
+
+每台相机必须有对应当前镜头、焦距、分辨率的独立内参文件：
+
+```text
+~/.ros/camera_info/hikrobot_DB2163742.yaml
+~/.ros/camera_info/hikrobot_DB2163749.yaml
+~/.ros/camera_info/hikrobot_DB2163739.yaml
+```
+
+三相机外参默认文件：
+
+```text
+/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml
+```
+
+移动相机、改变镜头焦距/对焦、改变分辨率或重新标定内参后，必须重新标定外参。
+
+### 3.2 11 个 AprilTag
+
+使用 `tag36h11`，有效黑框边长配置为 `16 mm`，不包含外围白边。
+
+| Tag ID | 机位 | 固定位置或运动件 |
+|---:|---|---|
+| 0 | 正面 | 掌壳固定基准 |
+| 1 | 正面 | 拇指 CMC 后连杆 |
+| 2 | 正面 | 拇指 MCP 后连杆 |
+| 3 | 正面 | 拇指 IP 后末节 |
+| 4 | 侧面 | 掌壳侧面固定基准 |
+| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
+| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
+| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
+| 8 | 上面 | 掌壳或底座固定基准 |
+| 9 | 上面 | 拇指 CMC yaw 运动件 |
+| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
+
+Tag 必须固定在刚性件上，不能跨关节、贴在软胶上、在运动中翘起或移动。Tag 的平面内旋转不要求贴正，但整个标定会话中安装姿态必须保持不变。
+
+## 4. 左右手配置差异
+
+左右手使用同一套拟合、留出验证和 URDF 写入算法，但使用独立的参考指、电机和镜像避挡姿态。
+
+| 项目 | 左手 | 右手 |
+|---|---|---|
+| 四指参考源 | 食指 `index` | 小指 `pinky` |
+| 扫描电机顺序 | `0/5/15/6/1/16/10` | `0/5/15/9/4/19/10` |
+| 参考指 MCP roll | 电机 6 | 电机 9 |
+| 参考指 MCP pitch | 电机 1 | 电机 4 |
+| 参考指 PIP | 电机 16 | 电机 19 |
+| 参考指 roll 避挡 | 其他侧摆电机置 0 | 其他侧摆电机置 255 |
+| 电机 0 辅助姿态 | 使用基准姿态 | 电机 5、10 固定 255 |
+
+两侧的非零拇指静态零偏都必须由各自当前会话的数据计算，不共享任何标定角度。
+
+## 5. 扫描任务和运动策略
+
+### 5.1 固定基准命令
+
+调用 `/g20_calibration/start` 后，程序先下发并确认以下 20 通道姿态：
+
+```text
+[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+ 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+```
+
+普通关节以命令 `255` 为动态曲线零点；四指 MCP 侧摆以命令 `127` 为动态曲线零点。
+
+### 5.2 七个扫描任务
+
+每个任务执行 3 轮 `255→0→255`，即每个任务 6 个方向。总计：
+
+```text
+7 个任务 × 3 轮 × 2 个方向 = 42 个扫描方向
+```
+
+| 顺序 | 任务 | 电机 | 机位 | 同时拟合 |
+|---:|---|---:|---|---|
+| 1 | 拇指 CMC pitch | 0 | 正面 | `thumb_cmc_pitch` |
+| 2 | 拇指 CMC roll | 5 | 正面 | `thumb_cmc_roll` |
+| 3 | 拇指 MCP | 15 | 正面 | `thumb_mcp`、被动 `thumb_ip` |
+| 4 | 参考指 MCP roll | 左6/右9 | 正面 | 参考指 `mcp_roll` |
+| 5 | 参考指 MCP pitch | 左1/右4 | 侧面 | 参考指 `mcp_pitch` |
+| 6 | 参考指 PIP | 左16/右19 | 侧面 | 参考指 `pip`、被动 `dip` |
+| 7 | 拇指 CMC yaw | 10 | 上面 | `thumb_cmc_yaw` |
+
+特殊辅助姿态：
+
+- 扫描拇指 yaw 时，电机 5 固定为 `145`，避免 Tag 9 姿态过斜；
+- 右手扫描电机 0 时，电机 5 和 10 固定为 `255`；
+- 扫描参考指 MCP roll 时，其余三指侧摆移到对应左右手避挡端；
+- 当前方向重试和人工恢复时，辅助姿态保持一致。
+
+### 5.3 标定速度
+
+G20 SDK 使用五指速度数组：
+
+- 常规速度：`15`；
+- 参考指 MCP roll：参考指速度 `5`；
+- 参考指 MCP pitch/PIP：参考指速度 `10`；
+- 自动重试速度比例：`80% / 60% / 50%`，最低速度不低于 `3`。
+
+## 6. 轨迹和关节轴拟合
+
+### 6.1 时间戳配对
+
+每帧 Tag 图像与 20 通道机械手状态按时间戳配对，默认最大允许偏差为 `50 ms`。标定曲线按实际命令分箱，但已确认的固件端点饱和反馈可以归入对应的命令端点分箱。
+
+每个扫描方向至少需要：
+
+- 40 帧同步有效数据；
+- 覆盖至少 240 个 u8；
+- 至少 32 个有效整数分箱；
+- 相邻有效分箱最大间隔不超过 16；
+- 同时包含命令 0 和 255 端点。
+
+### 6.2 动态角度曲线
+
+程序使用父/子 Tag 的完整相对旋转轨迹拟合关节转角，分别拟合下降和上升方向，检查单调修正、正反程回差和三轮行程一致性，再生成按命令索引的 256 点 `angle_rad`。
+
+四指策略：
+
+- 左手实测食指动态曲线，继承给中指、无名指、小指；
+- 右手实测小指动态曲线，继承给食指、中指、无名指；
+- 继承仅用于动态命令—角度关系；
+- 不把参考指的静态装配偏差复制给其他独立电机。
+
+### 6.3 三维轴方向和轴线位置
+
+当前算法不直接用单帧平面 Tag PnP 姿态作为关节角：
+
+- 轴方向主要来自整段相对旋转的螺旋轴；
+- 斜视且三维运动平面可观的关节，保留旋转轴和中心圆轴的交叉检查；
+- 接近端视的关节使用相对姿态轴约束圆轨迹方向；
+- 轴线上一点由整段相对 SE(3) 的 `(I-R)p=t` 方程拟合；
+- 接近沿轴观察时，丢弃单目无法稳定确定的光轴深度，只使用图像平面内可观分量。
+
+### 6.4 IPPE 双分支处理
+
+每轮运动前在静止端点联合 8 帧选择整组最稳定的平面 Tag PnP 分支。侧面 Tag 4/5/6/7 还检查贴面法向一致性，避免选择低重投影误差但几何镜像的分支。
+
+## 7. 当前 V3 静态零位求解逻辑
+
+### 7.1 为什么不能用两条根轴线间距确定掌部旋转
+
+拇指 CMC roll 根轴和四指 MCP roll 根轴在 CAD 中近似平行。旧逻辑使用两条三维轴线的空间间距确定掌坐标系绕根轴的旋转，但单目 PnP 的固定深度偏差会改变这条间距方向，并被误算成稳定的拇指 roll 静态零偏。
+
+这种误差可以三轮高度重复，因此“重复性好”并不能证明绝对零位正确。
+
+### 7.2 V3 掌坐标系锚定
+
+当前 V3 使用：
+
+1. 行程更充分的根轴实测方向；
+2. 保持原始 CAD 直立的参考指 MCP pitch 实测轴方向；
+3. 两条根轴线位置只用于平移，不参与绕根轴旋转。
+
+左手使用食指 MCP pitch，右手使用小指 MCP pitch。该逻辑全部由当前会话轨迹计算，不包含按左右手或序列号写死的拇指角度。
+
+### 7.3 拇指零位依赖链
+
+拇指静态零位按可观测链逐级求解：
+
+- `thumb_cmc_yaw` 实测轴方向观测 `thumb_cmc_roll` 零位；
+- `thumb_cmc_pitch` 实测轴方向观测 `thumb_cmc_yaw` 零位；
+- `thumb_mcp` 实测轴线相位观测 `thumb_cmc_pitch` 零位；
+- 被动 `thumb_ip` 的浅圆弧只作为诊断，不能覆盖 `thumb_mcp` 的原始 CAD 静态零位。
+
+逐关节一维鲁棒求解可防止远端异常把已经确定的上游零位一起拖到边界。
+
+### 7.4 四指静态零位保护
+
+当前 11-Tag 布局只能直接观测一根参考指，不能证明四根独立电机具有相同绝对装配相位。因此：
+
+- 四指 MCP roll 静态修正固定为原始 CAD 0；
+- 四指 MCP pitch 静态修正固定为原始 CAD 0；
+- 四指 PIP 静态修正固定为原始 CAD 0；
+- `thumb_mcp` 静态修正固定为原始 CAD 0；
+- 这些关节的动态 256 点曲线仍然实测或继承。
+
+这里的“0”表示不修改原始 CAD `origin.rpy`，不是额外写入某台机械手的人工标定角度。
+
+## 8. 质量门限和留出验证
+
+### 8.1 预检门限
+
+| 指标 | 默认要求 |
+|---|---:|
+| 预检窗口 | 60 帧 |
+| 所需 Tag 同时有效率 | ≥95% |
+| 检测频率 | ≥15 Hz，正常应接近30 Hz |
+| Hamming | 0 |
+| Decision margin | ≥30 |
+| Tag 最小边长 | ≥30 px |
+| PnP 重投影 RMS | ≤1.5 px |
+| 图像—状态时间差 | ≤50 ms |
+
+### 8.2 轨迹和轴门限
+
+| 指标 | 默认要求 |
+|---|---:|
+| 主动关节旋转轴外 RMS | ≤2.5° |
+| 被动关节旋转轴外 RMS | ≤7.5° |
+| 三轮轴方向极差 | ≤0.75° |
+| 轴线径向 RMS | ≤3 mm |
+| SE(3) 轴线拟合 RMS | ≤1 mm |
+| 可观三维圆的姿态轴/圆轴夹角 | ≤1° |
+| 父子轴锥角几何不一致 | ≤5° |
+| 主动曲线三轮行程差 | ≤3° |
+| 被动曲线三轮行程差 | ≤10° |
+| 主动最大单调修正 | ≤2° |
+| 主动最大回差 | ≤5° |
+| 被动最大单调修正 | ≤3° |
+| 被动最大回差 | ≤7.5° |
+
+拇指可求解静态偏移默认安全范围为 `±20°`。四指静态偏移不由相机相位覆盖，保持原始 CAD。
+
+### 8.3 第三轮强制留出
+
+前两轮用于训练，第三轮必须作为独立留出验证：
+
+- 轨迹角度 MAE ≤1°；
+- 轨迹角度 P95 ≤2°；
+- 非零静态修正必须在第三轮优于原始 URDF；
+- 改善必须通过按三轮分组的 95% bootstrap 置信检查；
+- 最终再使用三轮全部数据重拟合正式结果。
+
+`validation_enabled:=false` 只关闭额外随机机械动作，不能关闭第三轮留出验证。
+
+## 9. 运动安全和端点处理
+
+默认端点容差为 `±2 u8`，只有经过实机确认的固件饱和端点使用专用容差：
+
+| 端点 | 专用容差 |
+|---|---:|
+| 拇指 yaw 电机10，命令0 | ±4 u8 |
+| 右手拇指 yaw 电机10，命令255 | ±5 u8，实机可能反馈250 |
+| 右手小指 PIP 电机19，命令0 | ±5 u8，实机可能反馈5 |
+
+运动保护：
+
+- 单方向扫描超时：90 秒；
+- 连续 8 秒没有至少 1 u8 的目标方向进展：立即暂停并保持当前位置；
+- 机械停滞不消耗遮挡/采样自动重试预算；
+- 发现摩擦、碰撞或仍在变化的反馈时，不要反复调用 `resume` 强推；
+- 只有确认是稳定固件端点时，才允许为该电机、该端点配置专用容差。
+
+## 10. 标定前准备
+
+### 10.1 构建和加载环境
+
+```bash
+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
+```
+
+每次修改代码并重新构建后，必须关闭旧标定进程，在新终端重新 `source install/setup.bash` 后启动。
+
+### 10.2 现场检查
+
+开始前确认：
+
+- `can0` 已启动；
+- MVS 客户端没有占用三台相机；
+- 三个内参文件和外参文件对应当前相机安装；
+- 11 个 Tag 固定、平整、全行程可见；
+- 机械手全行程没有碰撞；
+- 没有其他节点向同一只手发布位置命令；
+- 准备好随时断开电机电源；
+- 源 URDF 是原始 CAD 文件。
+
+## 11. 禁止运动预检
+
+### 11.1 右手预检
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=right \
+  serial_number:=G20_RIGHT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
+  can_interface:=can0 \
+  commands_enabled:=false
+```
+
+### 11.2 左手预检
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=left \
+  serial_number:=G20_LEFT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  can_interface:=can0 \
+  commands_enabled:=false
+```
+
+`commands_enabled:=false` 时不允许标定位置运动。完成检查后应关闭预检进程，再启动正式流程，避免相机和 SDK 被两个进程同时占用。
+
+## 12. 正式标定操作
+
+### 12.1 右手正式启动
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=right \
+  serial_number:=G20_RIGHT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
+  can_interface:=can0
+```
+
+### 12.2 左手正式启动
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=left \
+  serial_number:=G20_LEFT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  can_interface:=can0
+```
+
+### 12.3 查看状态
+
+另开一个终端：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 topic echo --once \
+  /g20_calibration/status_text \
+  --field data
+```
+
+需要连续监控时去掉 `--once`。
+
+查看三个校正画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/front/camera/image_rect
+
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/side/camera/image_rect
+
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/top/camera/image_rect
+```
+
+### 12.4 开始运动
+
+只有状态进入“等待开始”，三个机位均显示“就绪”、外参匹配、所需 Tag 无缺失、SDK 正常后，才调用一次：
+
+```bash
+ros2 service call \
+  /g20_calibration/start \
+  std_srvs/srv/Trigger {}
+```
+
+不要重复调用 `start`。程序会自动完成基准姿态、42 个扫描方向、拟合、第三轮留出验证、正式 JSON 和 URDF 写入。
+
+## 13. 暂停、恢复和终止
+
+### 13.1 手工暂停
+
+```bash
+ros2 service call \
+  /g20_calibration/pause \
+  std_srvs/srv/Trigger {}
+```
+
+暂停会保持当前实际位置，不会自动返回基准。
+
+### 13.2 恢复可恢复故障
+
+```bash
+ros2 service call \
+  /g20_calibration/resume \
+  std_srvs/srv/Trigger {}
+```
+
+恢复规则：
+
+- 只要求当前活动机位恢复就绪；
+- 当前失败方向会从起点完整重扫；
+- 关节拟合失败会清除当前失败关节数据并重扫该关节的 6 个方向；
+- 已经通过的其他关节数据保留；
+- 不要用 `start` 代替 `resume`。
+
+### 13.3 终止
+
+```bash
+ros2 service call \
+  /g20_calibration/abort \
+  std_srvs/srv/Trigger {}
+```
+
+终止会停止任务并保持当前位置，不主动移动机械手。
+
+## 14. 自动重试和失败分类
+
+| 失败类型 | 程序行为 | 操作建议 |
+|---|---|---|
+| 短时 Tag 丢失、同步中断、端点/分箱不足 | 自动重扫当前方向，最多3次 | 修正遮挡后必要时 `resume` |
+| 单轮/单关节轨迹拟合失败 | 优先重扫失败轮次；最多自动重采2轮 | 修正可见性或机械行程后 `resume` |
+| 电机8秒无进展 | 立即保持并暂停，不消耗采样重试 | 先排查机械问题或确认固件端点 |
+| 三轮轴方向或零位离散过大 | 当前关节结束后暂停 | 修正问题后 `resume` 重扫该关节 |
+| 零位触边、父子轴几何不一致、留出无改善 | 稳定模型失败，不自动重扫 | `resume` 被拒绝；修正根因后启动新会话 |
+| 操作员暂停 | 保持当前位置 | 确认安全后 `resume` |
+
+自动重试只改变采集速度和保持时间，不放宽最终质量门限。
+
+## 15. 常见状态问题
+
+### 15.1 预检只有约 3 Hz
+
+优先检查：
+
+- 是否通过正式 launch 启动，从而加载 Fast DDS 大图共享内存配置；
+- 是否还有旧相机或 MVS 客户端占用设备；
+- `camera_info` 和 `image_raw/image_rect` 是否都接近 30 Hz；
+- 是否重新 `source install/setup.bash`；
+- 是否存在多个图像查看或录制进程造成额外负载。
+
+不要通过降低 Tag 有效率门限绕过帧率问题。
+
+### 15.2 显示“缺失Tag=无”，但同时有效率不足
+
+“当前缺失”只表示最新帧；同时有效率是预检滑动窗口内所有必需 Tag 同帧有效的比例。等待窗口更新，或排查间歇性遮挡、反光、角点质量和 PnP 分支失败。
+
+### 15.3 电机目标255、反馈稳定250
+
+当前代码只对右手电机10的255端配置 `±5 u8`。其他电机不能因为一次卡滞而放宽。先确认反馈确实稳定在固件端点，且不存在摩擦或碰撞。
+
+### 15.4 被动 DIP 轴外残差过大
+
+被动关节允许更宽的单轴残差，但仍必须满足跨轮轴方向和第三轮留出。若 Tag、外参均正常，需检查耦合机构是否存在非理想运动、松动或采样过程中 PnP 分支变化；不要直接放宽最终门限。
+
+### 15.5 零位/URDF模型验证失败
+
+该失败表示当前稳定观测无法由“原始 CAD + 纯关节零位旋转”解释。重复运动通常不会修复，程序会拒绝 `resume`。应检查原始 URDF、Tag所在刚性件、相机外参身份和标定模型后启动新会话。
+
+## 16. 输出目录和文件
+
+默认会话目录：
+
+```text
+calibration_output/<序列号>/<YYYYMMDD_HHMMSS>/
+```
+
+主要文件：
+
+```text
+raw_samples.jsonl
+g20_<left|right>_<序列号>_calibration.json
+```
+
+修正 URDF 默认写入原始 URDF 所在目录：
+
+```text
+linkerhand_g20_<left|right>_zero_calibrated_<序列号>_<时间戳>.urdf
+```
+
+写入约束：
+
+- 每次都从原始 CAD URDF 生成；
+- 采用 `T_original × Rot(axis, offset)`；
+- 只修改通过验收的主动关节 `origin.rpy`；
+- 不修改 `origin.xyz`、`axis.xyz`、mesh、mimic、连杆长度或机械限位；
+- 不覆盖原始 URDF；
+- 失败时不生成正式 JSON 和正式修正 URDF。
+
+程序会拒绝名称包含 `zero_calibrated` 或已有校准时间戳特征的源 URDF，防止重复修正。
+
+## 17. 使用已有采样离线重放
+
+当完整 `raw_samples.jsonl` 已存在时，可以使用当前代码重新拟合和生成新产物，不连接相机、不发送机械手命令。
+
+先只读验证：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+python3 -m g20_thumb_apriltag_calibration.offline_replay \
+  calibration_output/G20_RIGHT_001/20260811_120146 \
+  --output-tag AXIS_FRAME_V3
+```
+
+确认报告 `passed: true` 后写入新产物：
+
+```bash
+python3 -m g20_thumb_apriltag_calibration.offline_replay \
+  calibration_output/G20_RIGHT_001/20260811_120146 \
+  --output-tag AXIS_FRAME_V3 \
+  --write
+```
+
+`--output-tag` 只允许安全文件名字符。离线重放拒绝覆盖已有 JSON、URDF 和报告。
+
+离线流程会额外验证：
+
+- 原始 URDF 哈希在处理前后不变；
+- 写出的 URDF 等价于求解的运动学修正；
+- URDF 未修改关节平移和机械限位；
+- 将修正 URDF 作为候选模型重新求解后，残余零偏不超过允许值；
+- 正式文件哈希与临时候选文件一致。
+
+## 18. 仿真运行
+
+加载修正 URDF 后，使用同次标定 JSON 将 20 通道 u8 命令映射为 21 个 URDF 关节角：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch g20_thumb_apriltag_calibration \
+  calibrated_joint_state_bridge.launch.py \
+  hand_type:=right \
+  calibration_file:=/home/lxp/projects/linkerhand_retarget_ros2/calibration_output/G20_RIGHT_001/<时间戳>/g20_right_G20_RIGHT_001_calibration.json
+```
+
+默认：
+
+- 订阅 `/cb_right_hand_control_cmd`；
+- 发布 `/sim/mujoco/g20/right/joint_state`；
+- 发布值只包含动态 `angle_rad`；
+- 不会再次叠加 URDF 静态零偏。
+
+启动前必须停止其他向同一仿真关节话题发布的桥接节点，避免多个发布者同时驱动模型。
+
+## 19. 当前右手 V3 样例结果
+
+会话：
+
+```text
+G20_RIGHT_001/20260811_120146
+```
+
+使用当前 V3 掌坐标系锚定逻辑离线重放后：
+
+| 关节 | 静态 URDF 修正 |
+|---|---:|
+| `thumb_cmc_roll` | `+3.833362°` |
+| `thumb_cmc_yaw` | `+1.310573°` |
+| `thumb_cmc_pitch` | `+1.896384°` |
+| `thumb_mcp` | `0°`，保留CAD |
+| 四指 MCP roll/pitch、PIP | `0°`，保留CAD |
+
+该次 roll 三轮估计为：
+
+```text
+3.825454° / 3.866482° / 3.907776°
+```
+
+这些值说明当前会话的重复性，不是新标定的固定目标。重新标定应使用新采样独立计算；如果结果明显偏离且无法通过跨轮/留出门限，程序应拒绝生成正式产物。
+
+对应 V3 文件：
+
+```text
+src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/
+  linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_20260811_120146_AXIS_FRAME_V3.urdf
+
+calibration_output/G20_RIGHT_001/20260811_120146/
+  g20_right_G20_RIGHT_001_calibration_AXIS_FRAME_V3.json
+  g20_right_G20_RIGHT_001_offline_validation_AXIS_FRAME_V3.json
+```
+
+## 20. 正式验收检查表
+
+正式使用新产物前逐项确认：
+
+- [ ] 标定使用原始 CAD URDF，而不是旧修正 URDF；
+- [ ] 三个相机序列号、内参身份和外参身份匹配；
+- [ ] 三个机位接近 30 Hz；
+- [ ] 所需 Tag 同时有效率达到 95%；
+- [ ] 42 个扫描方向全部完成；
+- [ ] 三轮轴方向极差、轨迹残差和回差通过；
+- [ ] 第三轮轨迹 MAE/P95 通过；
+- [ ] 非零零位在第三轮显著改善原始 URDF；
+- [ ] 状态为 `COMPLETE`；
+- [ ] 正式 JSON 和修正 URDF 均已生成；
+- [ ] 仿真加载的是新 URDF 和同次 JSON；
+- [ ] 仿真关节话题只有一个发布者；
+- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
+

docs/O30右手三相机标定与URDF修正操作说明.md

@@ -0,0 +1,157 @@
+# O30 右手三相机标定与 URDF 修正操作说明
+
+> 支持范围：O30 右手、20 个有效电机、20 个主动 URDF 关节。
+
+## 1. 固定输入
+
+O30 SDK 工程：
+
+```text
+/home/lxp/projects/linkerhand-o30-ros2
+```
+
+原始 CAD URDF：
+
+```text
+/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf
+```
+
+标定基准命令固定为：
+
+```text
+[0, 0, 255, 205, 165, 20,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+```
+
+20 个通道均按完整 `255→0→255` 行程扫描。Tag 0～10 的机位、ID 和
+父子连杆角色与 G20 右手标定保持一致。
+
+## 2. SDK 与 URDF 映射
+
+| SDK 下标 | SDK 名称 | URDF 关节 |
+|---:|---|---|
+| 0 | `thumb_roll` | `thumb_cmc_roll` |
+| 1 | `thumb_yaw` | `thumb_cmc_yaw` |
+| 2～5 | `index/middle/ring/little_yaw` | 四指 `mcp_roll` |
+| 6 | `thumb_root1` | `thumb_mcp` |
+| 7～10 | 四指 `root1` | 四指 `mcp_pitch` |
+| 11～14 | 四指 `root2` | 四指 `pip` |
+| 15 | `thumb_tip` | `thumb_ip` |
+| 16～19 | 四指 `tip` | 四指 `dip` |
+
+O30 没有 `thumb_cmc_pitch`，也没有 G20 的被动 IP/DIP；O30 JSON 中的
+20 个 URDF 关节全部标记为主动关节。
+
+## 3. 扫描任务
+
+右手以小指为四指参考源，共执行 8 个任务、每项 3 轮：
+
+| 顺序 | 关节 | 电机 | 机位 |
+|---:|---|---:|---|
+| 1 | `thumb_cmc_roll` | 0 | front |
+| 2 | `thumb_mcp` | 6 | front |
+| 3 | `thumb_ip` | 15 | front |
+| 4 | `pinky_mcp_roll` | 5 | front |
+| 5 | `pinky_mcp_pitch` | 10 | side |
+| 6 | `pinky_pip` | 14 | side |
+| 7 | `pinky_dip` | 19 | side |
+| 8 | `thumb_cmc_yaw` | 1 | top |
+
+总计 `8 × 3 × 2 = 48` 个唯一扫描方向。O30 每一轮固定按
+`0→255`、`255→0` 完成一次往返，因此单关节三轮均为 `0→255→0`，并在
+每轮第一个方向前重新初始化活动机位的PnP跟踪。状态中的计划进度只统计48个
+唯一方向，另行显示包含自动重扫在内的实际执行方向次数。扫描小指 `mcp_roll` 时，电机
+2/3/4 固定为 255，使未贴 Tag 的三指避开正面机位。
+
+O30 命令增大时 URDF 动态角度增大；这与 G20 的命令—角度方向相反，标定
+JSON 会保存单调非递减曲线，并在各电机自己的基准命令处严格归零。
+
+O30 的固件反馈端点允许使用独立于 G20 的到位死区：一般通道为 `±4 u8`；
+拇指 CMC 侧摆电机 0 在低速命令 `255` 端为 `±9 u8`；
+食指 MCP 侧摆电机 2 在命令 `255` 端为 `±8 u8`；
+O30 固件的速度 `0` 仍然很快，且实机位置-时间模式无法可靠连续往返，因此标定
+强制使用普通位置模式和最低内部速度 `o30_internal_speed_u8:=0`。标定节点只发送
+最终目标，O30 SDK 在独立的约 125 Hz 控制线程中生成单字节位置斜坡，避免相机/PnP
+计算延迟导致 3～5 u8 的补偿跳步。默认 `o30_command_full_range_seconds:=6.0`，
+完整走过 `0～255` 约需 6 秒，两个方向采用相同限速。暂停、终止或异常时立即把
+斜坡目标替换为当前反馈并恢复直控模式。该策略不改变安全扫描范围，也不改变四指
+MCP 侧摆静态零位固定为 CAD 0 的规则。拇指 CMC 侧摆使用 `1 u8` 细步，其余
+通道累计 `3 u8` 后再向固件更新目标；后者为每个目标留出保持时间，避免电机6等
+关节因持续刷新单格目标而一直不启动，总体斜率和正反向用时保持不变。
+拇指 MCP（电机 6）在命令 0 端实测稳定反馈为 7，因此该端使用 `±8 u8`。
+这些门限只判断固件是否已经稳定到位，不改变实际下发的 `0～255` 扫描范围。
+
+## 4. 静态 URDF 零位策略
+
+- 四指 `mcp_roll`：测量小指动态曲线，但四个关节的静态 URDF 零偏全部固定为 0。
+- 小指 `mcp_pitch`：可由下游 PIP 轴观测并通过留出验证后修正。
+- 小指 `pip`：可由下游 DIP 轴线相位观测并通过留出验证后修正。
+- 小指 `dip`、拇指 `ip`：没有下游观察关节，静态零偏固定为 0。
+- 食指、中指、无名指：动态曲线从小指曲线按各自基准命令重新归零；静态偏差不继承。
+- 拇指 `cmc_roll/cmc_yaw/mcp`：按相邻下游轴逐级求解并执行第三轮留出验证。
+
+修正 URDF 只改通过验证的关节 `origin.rpy`，不改 `origin.xyz`、`axis`、
+mesh、连杆长度和 CAD 限位。生成后的 URDF 不能作为下一次标定的源文件。
+
+## 5. 启动
+
+先保证 O30 SDK 已编译，并依次加载 ROS、O30 SDK 和本工程：
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source /home/lxp/projects/linkerhand-o30-ros2/install/setup.bash
+source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
+```
+
+预检（不运动）：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_model:=O30 hand_type:=right \
+  serial_number:=O30_RIGHT_001 \
+  source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
+  start_sdk:=false commands_enabled:=false
+```
+
+正式启动，蓝色/黑色厂商 CANFD 盒：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_model:=O30 hand_type:=right \
+  serial_number:=O30_RIGHT_001 \
+  source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
+  o30_comm_type:=libcanbus canfd_device:=0 o30_auto_setup:=false \
+  o30_internal_speed_u8:=0 o30_command_full_range_seconds:=6.0
+```
+
+三个机位预检通过后查看状态并启动：
+
+```bash
+ros2 topic echo /o30_calibration/status_text
+ros2 service call /o30_calibration/start std_srvs/srv/Trigger {}
+```
+
+暂停、继续和终止服务分别为：
+
+```text
+/o30_calibration/pause
+/o30_calibration/resume
+/o30_calibration/abort
+```
+
+## 6. 产物
+
+会话目录中生成：
+
+```text
+raw_samples.jsonl
+o30_right_<serial>_calibration.json
+```
+
+修正 URDF 默认生成在原始 URDF 同目录，文件名包含
+`zero_calibrated_<serial>_<timestamp>`。JSON 的动态角度必须和这次生成的修正
+URDF 配套使用；运行时不能再次叠加 `urdf_zero_offset_rad`。

src/g20_thumb_apriltag_calibration/README.md

@@ -0,0 +1,778 @@
+# G20 左右手与 O30 右手 AprilTag 标定
+
+O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
+profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
+[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
+
+## 三机位三维关节轴零位标定（schema v4）
+
+正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
+`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
+贴正；相机和Tag在一次标定中必须固定。默认绑定为：
+
+```text
+front = DB2163742，Tag 0/1/2/3/10
+side  = DB2163749，Tag 4/5/6/7
+top   = DB2163739，Tag 8/9
+```
+
+11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致：
+
+| ID | 机位 | 固定位置/运动件 |
+|---:|---|---|
+| 0 | 正面 | 正面掌壳固定基准 |
+| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
+| 2 | 正面 | 拇指 MCP 后连杆 |
+| 3 | 正面 | 拇指 IP 后末节 |
+| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
+| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
+| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
+| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
+| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
+| 9 | 上面 | 拇指 CMC yaw 运动件 |
+| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
+
+ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
+ID 9 必须在拇指横摆的完整行程中持续可见。
+贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
+
+每台相机必须有独立内参文件：
+
+```text
+~/.ros/camera_info/hikrobot_DB2163742.yaml
+~/.ros/camera_info/hikrobot_DB2163749.yaml
+~/.ros/camera_info/hikrobot_DB2163739.yaml
+```
+
+### 1. 一次性三相机外参
+
+三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用
+`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘：
+
+```bash
+mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_extrinsics.launch.py \
+  output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
+```
+
+启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换
+`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合
+RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、
+同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张
+PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动
+采集，移到新姿态后再自动采下一组。
+
+外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
+一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
+`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
+配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
+单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
+新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
+再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
+放宽。两组均得到
+至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
+
+```bash
+ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
+ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
+ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
+```
+
+采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终
+显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出
+`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过
+1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时
+绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式
+标定也不会运动。
+
+### 2. 预检和正式标定
+
+先使用禁止运动模式检查三个机位、外参、内参和标签：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=left \
+  serial_number:=G20_LEFT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  commands_enabled:=false
+```
+
+分别查看正式流程的三个画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/front/camera/image_rect
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/side/camera/image_rect
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_calibration/top/camera/image_rect
+```
+
+确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=left \
+  serial_number:=G20_LEFT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  can_interface:=can0
+```
+
+右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  three_camera_calibration.launch.py \
+  hand_type:=right \
+  serial_number:=G20_RIGHT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  can_interface:=can0
+```
+
+状态显示三个机位均“就绪”后只调用一次：
+
+```bash
+ros2 topic echo /g20_calibration/status_text
+ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
+```
+
+收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始
+第一条轨迹扫描：
+
+```text
+[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
+ 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+```
+
+左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描
+`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对
+四元数的旋转向量投影到三维拟合轴得到。
+轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立
+交叉检查并参与融合。轴线上一点则由整段
+相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接
+当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的
+光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
+严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承
+给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立
+电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留
+原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
+
+零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
+掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
+稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
+进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
+轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
+超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
+Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
+1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
+小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
+
+`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
+被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
+偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
+`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
+`angle_rad[256]` 实测轨迹。
+
+前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
+差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
+95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
+
+单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
+耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
+不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
+
+四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差
+仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用
+完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型
+之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承
+上游轴方向时不重复报告同一项跨轮轴失败。
+
+四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD
+的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成
+约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
+
+四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
+但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
+修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
+按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
+
+7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
+拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
+约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
+原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
+保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
+偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
+会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
+不会放宽正式结果的硬门限。
+
+生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
+`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
+JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
+
+坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
+重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
+端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
+连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
+暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
+完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
+拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
+只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
+失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
+仍保存在 `raw_samples.jsonl`。
+
+每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
+程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
+分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免
+错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立
+重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。
+再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。
+
+左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像，
+将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为
+`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用
+电机1/16（左）或4/19（右），参考指速度10。
+
+右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到
+255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部
+Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节
+的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手
+仍沿用原有正面机位和基准姿态。
+
+标定 `thumb_cmc_yaw`（电机10）时，程序将
+`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
+ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
+电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
+恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
+暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
+`baseline_command_u8` 不变。
+
+右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19
+的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍
+使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判
+为端点到达。
+
+右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端
+使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为
+电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。
+
+三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
+始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
+状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和
+验证完成、正式JSON与URDF成功写入之前不会显示100%。
+
+上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
+和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
+当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
+
+### 3. 输出
+
+通过后生成精简JSON和一个新URDF：
+
+```text
+calibration_output/G20_LEFT_001/<时间戳>/
+  g20_left_G20_LEFT_001_calibration.json
+src/.../g20_left/
+  linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
+
+calibration_output/G20_RIGHT_001/<时间戳>/
+  g20_right_G20_RIGHT_001_calibration.json
+src/.../g20_right/
+  linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
+```
+
+文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
+`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
+指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
+覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
+改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
+图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
+
+完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
+命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
+
+```bash
+python3 -m g20_thumb_apriltag_calibration.offline_replay \
+  calibration_output/G20_RIGHT_001/20260811_120146 \
+  --output-tag AXIS_FRAME_V3 \
+  --write
+```
+
+下面保留原有正面拇指独立标定说明和兼容入口。
+
+### 4. 修正URDF的运行时关节映射
+
+修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节
+`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点
+`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线
+用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节
+`JointState`（包括5个被动关节）：
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
+  hand_type:=right \
+  calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
+```
+
+默认订阅 `/cb_right_hand_control_cmd`，发布
+`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
+发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
+因此不会静默退回旧标定。
+
+该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`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 的实测有效边长（米），当前实物黑色正方形实测为
+  `16 mm`，因此配置为 `0.016`。
+  测量检测角点所围成的正方形边长，不包含外围白色留边。
+- 当前试标定允许四张 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-image-pipeline \
+  ros-jazzy-apriltag-ros \
+  ros-jazzy-apriltag-msgs \
+  ros-jazzy-camera-calibration \
+  python3-yaml
+
+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
+```
+
+相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在：
+
+```text
+/opt/MVS/lib/64/libMvCameraControl.so
+/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py
+```
+
+正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是
+`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举
+顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
+
+`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
+三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量
+加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz，
+大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。
+
+首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从
+`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时
+仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。
+
+先单独启动相机（不会连接机械手，也不会发送关节命令）：
+
+```bash
+ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
+  --remap __ns:=/camera/camera/color \
+  -p serial_number:=DB2163742 \
+  -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
+```
+
+测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：
+
+```bash
+ros2 topic hz /camera/camera/color/camera_info
+ros2 topic hz /camera/camera/color/image_raw
+```
+
+使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按
+实际标定板修改：
+
+```bash
+ros2 run camera_calibration cameracalibrator \
+  --size 8x6 --square 0.020 \
+  --camera_name hikrobot_front_DB2163742 \
+  --ros-args \
+  --remap image:=/camera/camera/color/image_raw \
+  --remap camera/set_camera_info:=/camera/camera/color/set_camera_info
+```
+
+在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会
+原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、
+分辨率和 ROI；改变任何一项都要重新标定。
+
+连接 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 \
+  camera_serial_number:=DB2163742 \
+  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 \
+  camera_serial_number:=DB2163742 \
+  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 使用完整的 1624×1240 校正画面。查看实际送入 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` 同时接受 MVS
+序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。
+
+海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益
+`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用
+`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定
+曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加
+`record_bag:=true`。
+
+默认对完整 1624×1240 原图进行畸变校正和 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/fastdds_large_images.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<dds>
+  <profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <transport_descriptors>
+      <transport_descriptor>
+        <transport_id>g20_udp_transport</transport_id>
+        <type>UDPv4</type>
+        <sendBufferSize>10485760</sendBufferSize>
+        <receiveBufferSize>10485760</receiveBufferSize>
+      </transport_descriptor>
+      <transport_descriptor>
+        <transport_id>g20_shm_transport</transport_id>
+        <type>SHM</type>
+        <segment_size>67108864</segment_size>
+        <port_queue_capacity>512</port_queue_capacity>
+        <healthy_check_timeout_ms>1000</healthy_check_timeout_ms>
+      </transport_descriptor>
+    </transport_descriptors>
+
+    <participant
+      profile_name="g20_large_image_participant"
+      is_default_profile="true">
+      <rtps>
+        <userTransports>
+          <transport_id>g20_udp_transport</transport_id>
+          <transport_id>g20_shm_transport</transport_id>
+        </userTransports>
+        <useBuiltinTransports>false</useBuiltinTransports>
+      </rtps>
+    </participant>
+  </profiles>
+</dds>

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.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3]
+      frames: [tag_t0, tag_t3, tag_t4, tag_t5]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+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.016, 0.016, 0.016, 0.016]

src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml

@@ -0,0 +1,132 @@
+/**:
+  ros__parameters:
+    command_topic: /g20/cb_left_hand_control_cmd
+    state_topic: /g20/cb_left_hand_state
+    info_topic: /g20/cb_left_hand_info
+    setting_topic: /g20/cb_hand_setting_cmd
+    front_camera_info_topic: /g20_calibration/front/camera/camera_info
+    front_detections_topic: /g20_calibration/front/apriltag/detections
+    side_camera_info_topic: /g20_calibration/side/camera/camera_info
+    side_detections_topic: /g20_calibration/side/apriltag/detections
+    top_camera_info_topic: /g20_calibration/top/camera/camera_info
+    top_detections_topic: /g20_calibration/top/apriltag/detections
+
+    # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
+    baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    normal_calibration_speed: 15
+    index_roll_calibration_speed: 5
+    index_flex_calibration_speed: 10
+    # O30固件速度0仍很快，标定时使用最低内部速度，并由SDK位置斜坡控制平均速度。
+    o30_internal_speed_u8: 0
+    # O30 SDK位置斜坡完整走过0～255所需时间；仅O30使用，G20不受影响。
+    o30_command_full_range_seconds: 6.0
+    speed_setting_settle_seconds: 0.25
+
+    # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
+    tag_size_m: 0.016
+    repetitions: 3
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    minimum_detection_hz: 15.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+
+    pnp_maximum_reprojection_error_px: 1.5
+    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
+    pnp_tracker_reset_seconds: 5.0
+    # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
+    pnp_group_initialization_frames: 8
+    # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
+    pnp_group_normal_alignment_scale_deg: 5.0
+    pnp_group_maximum_normal_alignment_deg: 15.0
+    top_pnp_invalid_reset_seconds: 1.0
+    # 三维位姿必须与实测20通道状态严格按时间戳配对。
+    maximum_state_image_skew_ms: 50.0
+
+    axis_maximum_plane_rms_m: 0.003
+    # 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
+    # 轴线一致性仍沿用严格检查。
+    passive_axis_maximum_plane_rms_m: 0.004
+    axis_maximum_radial_rms_m: 0.003
+    # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
+    axis_maximum_pose_line_rms_m: 0.001
+    # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
+    # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
+    axis_maximum_rotation_circle_difference_deg: 1.0
+    # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
+    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
+    active_maximum_rotation_orthogonal_rms_deg: 2.5
+    passive_maximum_rotation_orthogonal_rms_deg: 7.5
+    zero_maximum_axis_cycle_difference_deg: 0.75
+    # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
+    zero_maximum_axis_cone_mismatch_deg: 5.0
+    zero_maximum_offset_deg: 20.0
+    # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
+    zero_finger_maximum_offset_deg: 3.0
+
+    endpoint_tolerance_u8: 2.0
+    # O30实机在多数目标处会稳定相差最多4；仅O30使用，不改变G20门限。
+    o30_endpoint_tolerance_u8: 4.0
+    # O30拇指CMC侧摆（电机0）低速命令255端实测稳定反馈为247；该端使用±9。
+    o30_thumb_cmc_roll_255_endpoint_tolerance_u8: 9.0
+    # O30食指MCP侧摆（电机2）命令255端实测稳定反馈为248；该端使用±8。
+    o30_index_mcp_roll_255_endpoint_tolerance_u8: 8.0
+    # O30拇指MCP（电机6）命令0端实测稳定反馈为7；该端使用±8。
+    o30_thumb_mcp_zero_endpoint_tolerance_u8: 8.0
+    # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
+    thumb_yaw_zero_endpoint_tolerance_u8: 4.0
+    # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
+    right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
+    # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
+    pinky_pip_zero_endpoint_tolerance_u8: 5.0
+    endpoint_hold_seconds: 0.5
+    baseline_hold_seconds: 0.5
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
+    # 这类故障不进入遮挡/超时的三次自动重扫。
+    motor_stall_timeout_seconds: 8.0
+    motor_stall_minimum_progress_u8: 1.0
+    invalid_timeout_seconds: 3.0
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
+    automatic_sweep_retry_limit: 3
+    # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
+    automatic_fit_retry_limit: 2
+    automatic_motion_retry_limit: 2
+    # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
+    provisional_warning_ratio: 1.25
+    retry_minimum_speed: 3
+    retry_speed_scales: [0.8, 0.6, 0.5]
+    retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
+
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.003
+    trajectory_minimum_arc_deg: 15.0
+    # 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
+    image_trajectory_maximum_radial_rms_px: 2.0
+    image_trajectory_maximum_radial_p95_px: 3.5
+    image_trajectory_minimum_radius_px: 20.0
+    trajectory_maximum_cycle_travel_difference_deg: 3.0
+    passive_maximum_cycle_travel_difference_deg: 10.0
+    maximum_monotonic_correction_deg: 2.0
+    maximum_hysteresis_deg: 5.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    passive_maximum_hysteresis_deg: 7.5
+
+    # 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
+    validation_enabled: false
+    validation_command_count: 3
+    validation_frames: 10
+    validation_seed: 20260804
+    validation_timeout_seconds: 20.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0

src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml

@@ -0,0 +1,62 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7]
+      frames: [side_base, index_mcp, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml

@@ -0,0 +1,62 @@
+/o30_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o30_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7]
+      frames: [side_base, pinky_mcp, pinky_pip, pinky_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+/o30_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__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,714 @@
+"""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
+    )
+
+
+def update_pnp_reset_watchdog(
+    *,
+    detection_good: bool,
+    pnp_valid: bool,
+    now: float,
+    invalid_since: float | None,
+    reset_after_seconds: float,
+) -> tuple[float | None, bool]:
+    """Track continuous PnP-only failures and request a throttled reset."""
+    reset_after = float(reset_after_seconds)
+    if reset_after <= 0.0:
+        raise ValueError("reset_after_seconds must be positive")
+    if not detection_good or pnp_valid:
+        return None, False
+    since = float(now) if invalid_since is None else float(invalid_since)
+    if float(now) - since >= reset_after:
+        # Start a new interval so a permanently bad view is not reset on every
+        # frame. The next valid frame clears the interval.
+        return float(now), True
+    return since, False
+
+
+def required_resume_views(active_view: str | None) -> tuple[str, ...]:
+    """Require only the active view on resume; start still checks all views."""
+    all_views = ("front", "side", "top")
+    if active_view is None:
+        return all_views
+    view = str(active_view)
+    if view not in all_views:
+        raise ValueError(f"unknown calibration view: {view}")
+    return (view,)
+
+
+@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/alignment_view.py

@@ -0,0 +1,358 @@
+"""Visual roll-alignment aid for one G20 calibration camera."""
+
+from __future__ import annotations
+
+from collections import deque
+import math
+import time
+from typing import Any, Mapping, Sequence
+
+from apriltag_msgs.msg import AprilTagDetectionArray
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+
+from .full_hand import VIEW_TAGS
+from .hikrobot_camera import configure_fastdds_large_image_transport
+from .zero_calibration import detect_reference_alignment_line
+
+
+VIEWS = ("front", "side", "top")
+
+
+def summarize_alignment_measurements(
+    measurements: Sequence[Mapping[str, Any] | None],
+) -> dict[str, Any] | None:
+    """Return a median-smoothed physical reference-line measurement."""
+    valid = [measurement for measurement in measurements if measurement]
+    if not valid:
+        return None
+    return {
+        "line_xyxy_px": np.median(
+            np.asarray(
+                [measurement["line_xyxy_px"] for measurement in valid],
+                dtype=float,
+            ),
+            axis=0,
+        ).tolist(),
+        "angle_rad": float(
+            np.median(
+                [float(measurement["angle_rad"]) for measurement in valid]
+            )
+        ),
+        "vertical_offset_px": float(
+            np.median(
+                [
+                    float(measurement["vertical_offset_px"])
+                    for measurement in valid
+                ]
+            )
+        ),
+        "detected_frames": len(valid),
+        "window_frames": len(measurements),
+    }
+
+
+class G20CameraAlignmentView(Node):
+    """Publish a red/blue roll aid based on a physical scene edge."""
+
+    def __init__(self) -> None:
+        """Configure one view without taking ownership of hand commands."""
+        super().__init__("g20_camera_alignment_view")
+        self.declare_parameter("view", "front")
+        view = str(self.get_parameter("view").value).strip().lower()
+        if view not in VIEWS:
+            raise ValueError(f"view must be one of {VIEWS}")
+        self.view = view
+
+        namespace = f"/g20_calibration/{view}"
+        self.required_tag_ids = {
+            int(value) for value in VIEW_TAGS[view].values()
+        }
+        self.declare_parameter("image_topic", f"{namespace}/camera/image_rect")
+        self.declare_parameter(
+            "detections_topic", f"{namespace}/apriltag/detections"
+        )
+        self.declare_parameter("reference_y_ratio", 0.90)
+        self.declare_parameter("roi_y_min_ratio", 0.55)
+        self.declare_parameter("roi_y_max_ratio", 0.98)
+        self.declare_parameter("minimum_line_length_ratio", 0.30)
+        self.declare_parameter("maximum_candidate_angle_deg", 15.0)
+        self.declare_parameter("maximum_alignment_error_deg", 0.5)
+        self.declare_parameter("maximum_vertical_offset_px", 12.0)
+        self.declare_parameter("maximum_hamming", 0)
+        self.declare_parameter("minimum_decision_margin", 20.0)
+        self.declare_parameter("minimum_edge_pixels", 20.0)
+        self.declare_parameter("smoothing_frames", 10)
+        self.declare_parameter("maximum_line_age_seconds", 1.0)
+        self.declare_parameter("maximum_tag_age_seconds", 1.0)
+        self.declare_parameter("maximum_publish_rate_hz", 10.0)
+        self.declare_parameter("output_scale", 0.75)
+
+        def value(name: str) -> Any:
+            return self.get_parameter(name).value
+
+        self.image_topic = str(value("image_topic"))
+        self.detections_topic = str(value("detections_topic"))
+        self.reference_y_ratio = float(value("reference_y_ratio"))
+        self.roi_y_min_ratio = float(value("roi_y_min_ratio"))
+        self.roi_y_max_ratio = float(value("roi_y_max_ratio"))
+        self.minimum_line_length_ratio = float(
+            value("minimum_line_length_ratio")
+        )
+        self.maximum_candidate_angle_rad = math.radians(
+            float(value("maximum_candidate_angle_deg"))
+        )
+        self.maximum_alignment_error_rad = math.radians(
+            float(value("maximum_alignment_error_deg"))
+        )
+        self.maximum_vertical_offset_px = float(
+            value("maximum_vertical_offset_px")
+        )
+        self.maximum_hamming = int(value("maximum_hamming"))
+        self.minimum_decision_margin = float(value("minimum_decision_margin"))
+        self.minimum_edge_pixels = float(value("minimum_edge_pixels"))
+        self.maximum_line_age_seconds = float(
+            value("maximum_line_age_seconds")
+        )
+        self.maximum_tag_age_seconds = float(value("maximum_tag_age_seconds"))
+        self.maximum_publish_rate_hz = float(value("maximum_publish_rate_hz"))
+        self.output_scale = float(value("output_scale"))
+        smoothing_frames = int(value("smoothing_frames"))
+
+        if not (
+            0.0
+            <= self.roi_y_min_ratio
+            < self.reference_y_ratio
+            < self.roi_y_max_ratio
+            <= 1.0
+        ):
+            raise ValueError(
+                "ratios must satisfy 0 <= roi_min < reference < roi_max <= 1"
+            )
+        if not 0.0 < self.minimum_line_length_ratio <= 1.0:
+            raise ValueError("minimum_line_length_ratio must be in (0, 1]")
+        if not (
+            0.0
+            < self.maximum_alignment_error_rad
+            < self.maximum_candidate_angle_rad
+            < math.pi / 2.0
+        ):
+            raise ValueError(
+                "angle limits must satisfy 0 < alignment < candidate < 90"
+            )
+        if self.maximum_vertical_offset_px <= 0.0:
+            raise ValueError("maximum_vertical_offset_px must be positive")
+        if smoothing_frames < 1:
+            raise ValueError("smoothing_frames must be positive")
+        if self.maximum_line_age_seconds <= 0.0:
+            raise ValueError("maximum_line_age_seconds must be positive")
+        if self.maximum_tag_age_seconds <= 0.0:
+            raise ValueError("maximum_tag_age_seconds must be positive")
+        if self.maximum_publish_rate_hz <= 0.0:
+            raise ValueError("maximum_publish_rate_hz must be positive")
+        if not 0.1 <= self.output_scale <= 1.0:
+            raise ValueError("output_scale must be in [0.1, 1.0]")
+
+        self.bridge = CvBridge()
+        self.line_history: deque[dict[str, Any] | None] = deque(
+            maxlen=smoothing_frames
+        )
+        self.last_line_at = 0.0
+        self.latest_tag_corners: dict[int, np.ndarray] = {}
+        self.latest_tag_at: dict[int, float] = {}
+        self.last_publish_at = 0.0
+        self.publisher = self.create_publisher(
+            Image, "~/image", qos_profile_sensor_data
+        )
+        self.create_subscription(
+            AprilTagDetectionArray,
+            self.detections_topic,
+            self._detections_callback,
+            qos_profile_sensor_data,
+        )
+        self.create_subscription(
+            Image,
+            self.image_topic,
+            self._image_callback,
+            qos_profile_sensor_data,
+        )
+        self.get_logger().info(
+            f"{view} alignment view uses physical long-edge detection; "
+            f"Tag orientation is ignored; input={self.image_topic}; "
+            f"output={self.get_name()}/image"
+        )
+
+    def _detections_callback(self, message: AprilTagDetectionArray) -> None:
+        now = time.monotonic()
+        for detection in message.detections:
+            tag_id = int(detection.id)
+            if tag_id not in self.required_tag_ids:
+                continue
+            corners = np.asarray(
+                [
+                    [float(point.x), float(point.y)]
+                    for point in detection.corners
+                ],
+                dtype=float,
+            )
+            if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+                continue
+            edges = np.linalg.norm(
+                corners - np.roll(corners, -1, axis=0), axis=1
+            )
+            if (
+                int(detection.hamming) > self.maximum_hamming
+                or float(detection.decision_margin)
+                < self.minimum_decision_margin
+                or float(np.mean(edges)) < self.minimum_edge_pixels
+            ):
+                continue
+            self.latest_tag_corners[tag_id] = corners
+            self.latest_tag_at[tag_id] = now
+
+    def _draw_tags(self, image: np.ndarray, now: float) -> None:
+        for tag_id in sorted(self.required_tag_ids):
+            corners = self.latest_tag_corners.get(tag_id)
+            detected_at = self.latest_tag_at.get(tag_id, 0.0)
+            if (
+                corners is None
+                or now - detected_at > self.maximum_tag_age_seconds
+            ):
+                continue
+            points = np.rint(corners * self.output_scale).astype(np.int32)
+            cv2.polylines(image, [points], True, (0, 220, 0), 2)
+            centre = np.rint(np.mean(points, axis=0)).astype(int)
+            cv2.putText(
+                image,
+                f"ID {tag_id}",
+                (int(centre[0]) + 5, int(centre[1]) - 7),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.55,
+                (0, 220, 0),
+                2,
+            )
+
+    def _image_callback(self, message: Image) -> None:
+        # Avoid conversion and Hough work until an image viewer subscribes.
+        if self.publisher.get_subscription_count() < 1:
+            return
+        now = time.monotonic()
+        if now - self.last_publish_at < 1.0 / self.maximum_publish_rate_hz:
+            return
+        self.last_publish_at = now
+        try:
+            image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except Exception as error:
+            self.get_logger().warning(
+                f"alignment image conversion failed: {error}"
+            )
+            return
+        if self.output_scale != 1.0:
+            image = cv2.resize(
+                image,
+                None,
+                fx=self.output_scale,
+                fy=self.output_scale,
+                interpolation=cv2.INTER_AREA,
+            )
+
+        height, width = image.shape[:2]
+        reference_y = self.reference_y_ratio * float(height - 1)
+        detected = detect_reference_alignment_line(
+            image,
+            reference_y_px=reference_y,
+            roi_y_min_ratio=self.roi_y_min_ratio,
+            roi_y_max_ratio=self.roi_y_max_ratio,
+            minimum_length_ratio=self.minimum_line_length_ratio,
+            maximum_candidate_angle_rad=self.maximum_candidate_angle_rad,
+        )
+        self.line_history.append(detected)
+        if detected is not None:
+            self.last_line_at = now
+        measurement = summarize_alignment_measurements(self.line_history)
+        if now - self.last_line_at > self.maximum_line_age_seconds:
+            measurement = None
+
+        red_y = int(round(reference_y))
+        cv2.line(
+            image,
+            (15, red_y),
+            (max(15, width - 15), red_y),
+            (0, 0, 255),
+            4,
+        )
+        if measurement is not None:
+            line = np.rint(measurement["line_xyxy_px"]).astype(int)
+            blue_ok, blue_start, blue_end = cv2.clipLine(
+                (0, 0, width, height),
+                (int(line[0]), int(line[1])),
+                (int(line[2]), int(line[3])),
+            )
+            if blue_ok:
+                cv2.line(image, blue_start, blue_end, (255, 0, 0), 3)
+            angle_rad = float(measurement["angle_rad"])
+            offset_px = float(measurement["vertical_offset_px"])
+            aligned = bool(
+                abs(angle_rad) <= self.maximum_alignment_error_rad
+                and abs(offset_px) <= self.maximum_vertical_offset_px
+            )
+            status = "ALIGNED" if aligned else "ADJUST CAMERA"
+            status_text = (
+                f"{self.view.upper()}  red-blue "
+                f"{math.degrees(angle_rad):+.2f} deg  "
+                f"dy {offset_px:+.1f}px  {status}"
+            )
+            status_color = (0, 220, 0) if aligned else (0, 165, 255)
+        else:
+            status_text = (
+                f"{self.view.upper()}  PHYSICAL REFERENCE LINE NOT DETECTED"
+            )
+            status_color = (0, 165, 255)
+        cv2.putText(
+            image,
+            status_text,
+            (20, 34),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.72,
+            status_color,
+            2,
+        )
+        self._draw_tags(image, now)
+        cv2.putText(
+            image,
+            "RED=target  BLUE=physical edge  GREEN=Tags (angle ignored)",
+            (20, max(64, height - 24)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.60,
+            (255, 255, 255),
+            2,
+        )
+        output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
+        output.header = message.header
+        self.publisher.publish(output)
+
+
+def main(args: list[str] | None = None) -> None:
+    """Run the single-view alignment helper."""
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: G20CameraAlignmentView | None = None
+    try:
+        node = G20CameraAlignmentView()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,288 @@
+"""Map supported-hand u8 commands to URDF angles using calibration JSON.
+
+The static encoder-zero corrections in ``zero_angles`` are already baked into
+the corrected URDF joint origins.  This bridge therefore publishes only the
+dynamic ``angle_rad`` values and never adds the static offsets a second time.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .full_hand import get_hand_calibration_profile, validate_compact_payload
+
+
+G20_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",
+)
+
+# Match the stable ordering used by the existing MuJoCo bridge.  JointState
+# consumers must use names, but retaining the ordering also keeps logs and
+# direct comparisons deterministic.
+G20_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "index_dip",
+    "index_mcp_pitch",
+    "index_mcp_roll",
+    "index_pip",
+    "middle_dip",
+    "middle_mcp_pitch",
+    "middle_mcp_roll",
+    "middle_pip",
+    "pinky_dip",
+    "pinky_mcp_pitch",
+    "pinky_mcp_roll",
+    "pinky_pip",
+    "ring_dip",
+    "ring_mcp_pitch",
+    "ring_mcp_roll",
+    "ring_pip",
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_ip",
+    "thumb_mcp",
+)
+
+O30_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_roll",
+    "thumb_yaw",
+    "index_yaw",
+    "middle_yaw",
+    "ring_yaw",
+    "little_yaw",
+    "thumb_root1",
+    "index_root1",
+    "middle_root1",
+    "ring_root1",
+    "little_root1",
+    "index_root2",
+    "middle_root2",
+    "ring_root2",
+    "little_root2",
+    "thumb_tip",
+    "index_tip",
+    "middle_tip",
+    "ring_tip",
+    "little_tip",
+)
+
+O30_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_mcp",
+    "thumb_ip",
+    "index_mcp_roll",
+    "index_mcp_pitch",
+    "index_pip",
+    "index_dip",
+    "middle_mcp_roll",
+    "middle_mcp_pitch",
+    "middle_pip",
+    "middle_dip",
+    "ring_mcp_roll",
+    "ring_mcp_pitch",
+    "ring_pip",
+    "ring_dip",
+    "pinky_mcp_roll",
+    "pinky_mcp_pitch",
+    "pinky_pip",
+    "pinky_dip",
+)
+
+COMMAND_NAMES_BY_MODEL = {
+    "G20": G20_COMMAND_NAMES,
+    "O30": O30_COMMAND_NAMES,
+}
+URDF_JOINT_NAMES_BY_MODEL = {
+    "G20": G20_URDF_JOINT_NAMES,
+    "O30": O30_URDF_JOINT_NAMES,
+}
+
+
+class CalibratedCommandMapper:
+    """Validated, model/side-specific lookup from commands to URDF radians."""
+
+    def __init__(
+        self, payload: Mapping[str, Any], *, expected_side: str | None = None
+    ) -> None:
+        validate_compact_payload(payload)
+        side = str(payload["side"]).lower()
+        if expected_side is not None and side != str(expected_side).lower():
+            raise ValueError(
+                f"calibration side {side!r} does not match requested side "
+                f"{str(expected_side).lower()!r}"
+            )
+        quality = payload["quality"]
+        if quality.get("passed") is not True:
+            raise ValueError("calibration quality.passed must be true")
+        model = str(payload["model"]).upper()
+        profile = get_hand_calibration_profile(side, model)
+        urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model]
+        self.model = model
+        self.side = side
+        self.serial_number = str(payload["serial_number"])
+        self.command_names = COMMAND_NAMES_BY_MODEL[model]
+        self.urdf_joint_names = urdf_joint_names
+        self._motor_by_joint = {
+            name: int(profile.joint_specs[name].motor_index)
+            for name in urdf_joint_names
+        }
+        self._curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name]["angle_rad"]
+            )
+            for name in urdf_joint_names
+        }
+
+    @staticmethod
+    def _command_index(value: float) -> int:
+        command = float(value)
+        if not math.isfinite(command):
+            raise ValueError("command positions must be finite")
+        return max(0, min(255, int(math.floor(command + 0.5))))
+
+    def map_positions(
+        self, positions: Sequence[float], names: Sequence[str] = ()
+    ) -> tuple[float, ...]:
+        values = tuple(float(value) for value in positions)
+        if names:
+            if len(names) != len(values):
+                raise ValueError(
+                    "JointState names and positions must have equal length"
+                )
+            if len(set(names)) != len(names):
+                raise ValueError("JointState names must be unique")
+            by_name = dict(zip((str(name) for name in names), values))
+            missing = [name for name in self.command_names if name not in by_name]
+            if missing:
+                raise ValueError(
+                    f"{self.model} command is missing named channels: "
+                    + ",".join(missing)
+                )
+            command = tuple(by_name[name] for name in self.command_names)
+        else:
+            if len(values) != len(self.command_names):
+                raise ValueError(
+                    f"unnamed {self.model} command must contain exactly "
+                    f"{len(self.command_names)} positions"
+                )
+            command = values
+        indices = tuple(self._command_index(value) for value in command)
+        return tuple(
+            self._curves[name][indices[self._motor_by_joint[name]]]
+            for name in self.urdf_joint_names
+        )
+
+
+def load_calibrated_command_mapper(
+    calibration_file: str | Path, *, expected_side: str | None = None
+) -> CalibratedCommandMapper:
+    path = Path(calibration_file).expanduser().resolve()
+    if not path.is_file():
+        raise ValueError(f"calibration JSON does not exist: {path}")
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    return CalibratedCommandMapper(payload, expected_side=expected_side)
+
+
+class CalibratedJointStateBridge(Node):
+    def __init__(self) -> None:
+        super().__init__("g20_calibrated_joint_state_bridge")
+        self.declare_parameter("hand_model", "G20")
+        self.declare_parameter("hand_type", "right")
+        self.declare_parameter("calibration_file", "")
+        self.declare_parameter("input_topic", "")
+        self.declare_parameter("output_topic", "")
+
+        hand_model = str(self.get_parameter("hand_model").value).upper()
+        hand_type = str(self.get_parameter("hand_type").value).lower()
+        get_hand_calibration_profile(hand_type, hand_model)
+        calibration_file = str(self.get_parameter("calibration_file").value)
+        if not calibration_file:
+            raise ValueError("calibration_file is required")
+        self.mapper = load_calibrated_command_mapper(
+            calibration_file, expected_side=hand_type
+        )
+        if self.mapper.model != hand_model:
+            raise ValueError(
+                f"calibration model {self.mapper.model!r} does not match "
+                f"requested model {hand_model!r}"
+            )
+        input_topic = str(self.get_parameter("input_topic").value).strip()
+        output_topic = str(self.get_parameter("output_topic").value).strip()
+        self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
+        self.output_topic = (
+            output_topic
+            or f"/sim/mujoco/{hand_model.lower()}/{hand_type}/joint_state"
+        )
+        self.publisher = self.create_publisher(JointState, self.output_topic, 10)
+        self.subscription = self.create_subscription(
+            JointState, self.input_topic, self._command_callback, 10
+        )
+        self._last_error = ""
+        self.get_logger().info(
+            f"loaded {hand_type} {hand_model} calibration for "
+            f"{self.mapper.serial_number}: "
+            f"{self.input_topic} -> {self.output_topic}"
+        )
+
+    def _command_callback(self, command: JointState) -> None:
+        try:
+            positions = self.mapper.map_positions(command.position, command.name)
+        except ValueError as error:
+            message = str(error)
+            if message != self._last_error:
+                self.get_logger().error(message)
+                self._last_error = message
+            return
+        self._last_error = ""
+        result = JointState()
+        result.header = command.header
+        result.name = list(self.mapper.urdf_joint_names)
+        result.position = list(positions)
+        self.publisher.publish(result)
+
+
+def main(args: Sequence[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: CalibratedJointStateBridge | None = None
+    try:
+        node = CalibratedJointStateBridge()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

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/extrinsics.py

@@ -0,0 +1,227 @@
+"""Camera-extrinsic data model shared by calibration and runtime nodes."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation
+
+
+VIEWS: tuple[str, ...] = ("front", "side", "top")
+
+
+def camera_info_fingerprint(
+    *,
+    width: int,
+    height: int,
+    camera_matrix: Sequence[Sequence[float]] | Sequence[float],
+    distortion: Sequence[float] = (),
+    rectification: Sequence[float] = (),
+    projection: Sequence[float] = (),
+) -> str:
+    """Return a stable fingerprint for rectified image geometry."""
+    matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
+    payload = {
+        "width": int(width),
+        "height": int(height),
+        "camera_matrix": [round(float(value), 12) for value in matrix.flat],
+        "distortion": [round(float(value), 12) for value in distortion],
+        "rectification": [round(float(value), 12) for value in rectification],
+        "projection": [round(float(value), 12) for value in projection],
+    }
+    encoded = json.dumps(
+        payload, sort_keys=True, separators=(",", ":")
+    ).encode("utf-8")
+    return hashlib.sha256(encoded).hexdigest()
+
+
+def transform_matrix(
+    translation_xyz_m: Sequence[float],
+    quaternion_xyzw: Sequence[float],
+) -> np.ndarray:
+    translation = np.asarray(translation_xyz_m, dtype=float)
+    quaternion = np.asarray(quaternion_xyzw, dtype=float)
+    if translation.shape != (3,) or not np.all(np.isfinite(translation)):
+        raise ValueError("translation_xyz_m must contain three finite values")
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion_xyzw must contain four finite values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1.0e-12:
+        raise ValueError("quaternion_xyzw has zero norm")
+    result = np.eye(4, dtype=float)
+    result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
+    result[:3, 3] = translation
+    return result
+
+
+def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
+    value = np.asarray(matrix, dtype=float)
+    if value.shape != (4, 4) or not np.all(np.isfinite(value)):
+        raise ValueError("transform must be a finite 4x4 matrix")
+    return {
+        "translation_xyz_m": [float(item) for item in value[:3, 3]],
+        "quaternion_xyzw": [
+            float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
+        ],
+    }
+
+
+@dataclass(frozen=True)
+class CameraCalibrationIdentity:
+    serial_number: str
+    width: int
+    height: int
+    intrinsics_sha256: str
+
+
+@dataclass(frozen=True)
+class ThreeCameraExtrinsics:
+    """Transforms points from each camera optical frame into front optical."""
+
+    cameras: Mapping[str, CameraCalibrationIdentity]
+    front_from_view: Mapping[str, np.ndarray]
+    quality: Mapping[str, float]
+
+    def transform(self, view: str) -> np.ndarray:
+        if view not in self.front_from_view:
+            raise KeyError(f"extrinsics do not contain view {view}")
+        return np.asarray(self.front_from_view[view], dtype=float).copy()
+
+    def camera_matches(
+        self,
+        view: str,
+        *,
+        serial_number: str,
+        width: int,
+        height: int,
+        intrinsics_sha256: str,
+    ) -> bool:
+        expected = self.cameras.get(view)
+        return bool(
+            expected is not None
+            and expected.serial_number == str(serial_number)
+            and expected.width == int(width)
+            and expected.height == int(height)
+            and expected.intrinsics_sha256 == str(intrinsics_sha256)
+        )
+
+
+def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
+    if int(payload.get("schema_version", -1)) != 1:
+        raise ValueError("camera extrinsics schema_version must be 1")
+    if payload.get("reference_view") != "front":
+        raise ValueError("camera extrinsics reference_view must be front")
+    cameras = payload.get("cameras")
+    transforms = payload.get("front_from_view")
+    quality = payload.get("quality")
+    if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
+        raise ValueError("camera extrinsics must contain front/side/top cameras")
+    if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
+        raise ValueError("camera extrinsics must contain all three transforms")
+    if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
+        raise ValueError("camera extrinsics quality is not passed")
+    quality_limits = {
+        "reprojection_rms_px": 1.2,
+        "maximum_rotation_repeatability_deg": 0.3,
+        "maximum_translation_repeatability_m": 0.0015,
+    }
+    for key, limit in quality_limits.items():
+        value = float(quality.get(key, float("inf")))
+        if not np.isfinite(value) or value > limit:
+            raise ValueError(
+                f"camera extrinsics {key}={value} exceeds {limit}"
+            )
+    for key in ("front_side_captures", "front_top_captures"):
+        if int(quality.get(key, 0)) < 15:
+            raise ValueError(f"camera extrinsics {key} must be at least 15")
+    for view in VIEWS:
+        identity = cameras[view]
+        if not isinstance(identity, Mapping):
+            raise ValueError(f"{view} camera identity must be an object")
+        if not str(identity.get("serial_number", "")):
+            raise ValueError(f"{view} camera serial_number is missing")
+        if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
+            raise ValueError(f"{view} camera image dimensions are invalid")
+        fingerprint = str(identity.get("intrinsics_sha256", ""))
+        if len(fingerprint) != 64:
+            raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
+        transform = transforms[view]
+        if not isinstance(transform, Mapping):
+            raise ValueError(f"{view} transform must be an object")
+        matrix = transform_matrix(
+            transform.get("translation_xyz_m", ()),
+            transform.get("quaternion_xyzw", ()),
+        )
+        if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
+            raise ValueError("front_from_view.front must be identity")
+    serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
+    if len(set(serials)) != len(VIEWS):
+        raise ValueError("camera extrinsics serial numbers must be unique")
+
+
+def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
+    source = Path(path).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"camera extrinsics file does not exist: {source}")
+    with source.open("r", encoding="utf-8") as stream:
+        payload = yaml.safe_load(stream)
+    if not isinstance(payload, Mapping):
+        raise ValueError("camera extrinsics file must contain an object")
+    validate_extrinsics_payload(payload)
+    cameras = {
+        view: CameraCalibrationIdentity(
+            serial_number=str(payload["cameras"][view]["serial_number"]),
+            width=int(payload["cameras"][view]["width"]),
+            height=int(payload["cameras"][view]["height"]),
+            intrinsics_sha256=str(
+                payload["cameras"][view]["intrinsics_sha256"]
+            ),
+        )
+        for view in VIEWS
+    }
+    transforms = {
+        view: transform_matrix(
+            payload["front_from_view"][view]["translation_xyz_m"],
+            payload["front_from_view"][view]["quaternion_xyzw"],
+        )
+        for view in VIEWS
+    }
+    return ThreeCameraExtrinsics(
+        cameras=cameras,
+        front_from_view=transforms,
+        quality={
+            str(key): float(value) if isinstance(value, (int, float)) else value
+            for key, value in payload["quality"].items()
+        },
+    )
+
+
+def dump_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    cameras: Mapping[str, Mapping[str, Any]],
+    front_from_view: Mapping[str, Sequence[Sequence[float]]],
+    quality: Mapping[str, Any],
+) -> None:
+    payload = {
+        "schema_version": 1,
+        "reference_view": "front",
+        "cameras": {view: dict(cameras[view]) for view in VIEWS},
+        "front_from_view": {
+            view: matrix_payload(front_from_view[view]) for view in VIEWS
+        },
+        "quality": dict(quality),
+    }
+    validate_extrinsics_payload(payload)
+    destination = Path(path).expanduser().resolve()
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    with temporary.open("w", encoding="utf-8") as stream:
+        yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
+    temporary.replace(destination)

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py

@@ -0,0 +1,644 @@
+"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
+
+from __future__ import annotations
+
+from ctypes import byref, cast, memset, POINTER, sizeof, string_at
+from dataclasses import dataclass
+import importlib
+import os
+from pathlib import Path
+import sys
+from threading import Event, Lock, Thread
+from typing import Any, Sequence
+from urllib.parse import unquote, urlparse
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+from sensor_msgs.srv import SetCameraInfo
+import yaml
+
+
+DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
+DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
+
+
+@dataclass(frozen=True)
+class DeviceDescriptor:
+    """Stable identity fields for one enumerated USB3 Vision camera."""
+
+    index: int
+    model: str
+    serial: str
+    guid: str
+
+
+@dataclass(frozen=True)
+class CameraCalibration:
+    """Validated subset of a standard ROS camera calibration YAML file."""
+
+    camera_name: str
+    width: int
+    height: int
+    distortion_model: str
+    d: tuple[float, ...]
+    k: tuple[float, ...]
+    r: tuple[float, ...]
+    p: tuple[float, ...]
+
+
+def decode_c_string(value: Any) -> str:
+    """Decode a null-terminated MVS SDK character array."""
+    raw = bytes(value).split(b"\0", 1)[0]
+    for encoding in ("utf-8", "gbk", "latin-1"):
+        try:
+            return raw.decode(encoding)
+        except UnicodeDecodeError:
+            continue
+    return raw.decode("latin-1", errors="replace")
+
+
+def select_device(
+    devices: Sequence[DeviceDescriptor],
+    selector: str,
+    expected_model: str,
+) -> DeviceDescriptor:
+    """Select exactly one camera by serial or GUID and check its model."""
+    if not devices:
+        raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
+    selector = selector.strip()
+    if selector:
+        matches = [
+            device
+            for device in devices
+            if selector in {device.serial, device.guid}
+        ]
+        if not matches:
+            available = ", ".join(
+                f"{device.model}:{device.serial}:{device.guid}"
+                for device in devices
+            )
+            raise RuntimeError(
+                f"camera selector {selector!r} was not found; "
+                f"available cameras: {available}"
+            )
+    elif len(devices) == 1:
+        matches = [devices[0]]
+    else:
+        available = ", ".join(device.serial for device in devices)
+        raise RuntimeError(
+            "camera selector is required when multiple cameras are present; "
+            f"available serials: {available}"
+        )
+    if len(matches) != 1:
+        raise RuntimeError(f"camera selector {selector!r} is not unique")
+    selected = matches[0]
+    if expected_model and expected_model not in selected.model:
+        raise RuntimeError(
+            f"camera {selected.serial} is {selected.model!r}, expected a model "
+            f"containing {expected_model!r}"
+        )
+    return selected
+
+
+def resolve_camera_info_path(url_or_path: str) -> Path | None:
+    """Resolve a plain path or file:// URL used for CameraInfo persistence."""
+    value = url_or_path.strip()
+    if not value:
+        return None
+    parsed = urlparse(value)
+    if parsed.scheme not in {"", "file"}:
+        raise ValueError(
+            "camera_info_url must be a filesystem path or file:// URL"
+        )
+    if parsed.scheme == "file":
+        if parsed.netloc not in {"", "localhost"}:
+            raise ValueError("remote file:// camera_info_url is not supported")
+        value = unquote(parsed.path)
+    return Path(value).expanduser().resolve()
+
+
+def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
+    section = mapping.get(name)
+    if not isinstance(section, dict):
+        raise ValueError(f"camera calibration is missing {name}")
+    data = section.get("data")
+    if not isinstance(data, list) or len(data) != length:
+        raise ValueError(f"{name}.data must contain exactly {length} values")
+    result = tuple(float(value) for value in data)
+    if not all(value == value and abs(value) != float("inf") for value in result):
+        raise ValueError(f"{name}.data contains a non-finite value")
+    return result
+
+
+def load_camera_calibration(path: Path) -> CameraCalibration:
+    """Load and validate a standard ROS camera calibration YAML file."""
+    mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
+    if not isinstance(mapping, dict):
+        raise ValueError("camera calibration YAML root must be a mapping")
+    width = int(mapping.get("image_width", 0))
+    height = int(mapping.get("image_height", 0))
+    if width <= 0 or height <= 0:
+        raise ValueError("camera calibration image dimensions must be positive")
+    distortion = mapping.get("distortion_coefficients")
+    if not isinstance(distortion, dict) or not isinstance(
+        distortion.get("data"), list
+    ):
+        raise ValueError(
+            "camera calibration is missing distortion_coefficients.data"
+        )
+    d = tuple(float(value) for value in distortion["data"])
+    calibration = CameraCalibration(
+        camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
+        width=width,
+        height=height,
+        distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
+        d=d,
+        k=_matrix_data(mapping, "camera_matrix", 9),
+        r=_matrix_data(mapping, "rectification_matrix", 9),
+        p=_matrix_data(mapping, "projection_matrix", 12),
+    )
+    if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
+        raise ValueError("camera calibration focal lengths must be positive")
+    if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
+        raise ValueError("camera calibration projection focal lengths must be positive")
+    return calibration
+
+
+def calibration_to_mapping(
+    camera_name: str, camera_info: CameraInfo
+) -> dict[str, Any]:
+    """Convert CameraInfo into the standard ROS YAML representation."""
+    return {
+        "image_width": int(camera_info.width),
+        "image_height": int(camera_info.height),
+        "camera_name": camera_name,
+        "camera_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.k],
+        },
+        "distortion_model": camera_info.distortion_model,
+        "distortion_coefficients": {
+            "rows": 1,
+            "cols": len(camera_info.d),
+            "data": [float(value) for value in camera_info.d],
+        },
+        "rectification_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.r],
+        },
+        "projection_matrix": {
+            "rows": 3,
+            "cols": 4,
+            "data": [float(value) for value in camera_info.p],
+        },
+    }
+
+
+def configure_fastdds_large_image_transport() -> Path:
+    """Select the package's large-image SHM profile before creating ROS nodes."""
+    from ament_index_python.packages import get_package_share_directory
+
+    profile = (
+        Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
+        / "config"
+        / "fastdds_large_images.xml"
+    )
+    if not profile.is_file():
+        raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
+    os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
+    return profile
+
+
+class HikrobotCameraNode(Node):
+    """Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
+
+    def __init__(self) -> None:
+        super().__init__("hikrobot_camera")
+        self._declare_parameters()
+        self._stop_event = Event()
+        self._camera_info_lock = Lock()
+        self._grab_thread: Thread | None = None
+        self._camera: Any | None = None
+        self._mvs: Any | None = None
+        self._sdk_initialized = False
+        self._handle_created = False
+        self._camera_opened = False
+        self._grabbing = False
+        self._closed = False
+        self._frame_count = 0
+        self._timeout_count = 0
+
+        self.selector = str(self.get_parameter("serial_number").value)
+        self.expected_model = str(self.get_parameter("expected_model").value)
+        self.frame_id = str(self.get_parameter("frame_id").value)
+        self.camera_name = str(self.get_parameter("camera_name").value)
+        self.image_width = int(self.get_parameter("image_width").value)
+        self.image_height = int(self.get_parameter("image_height").value)
+        if self.image_width <= 0 or self.image_height <= 0:
+            raise ValueError("image_width and image_height must be positive")
+        if float(self.get_parameter("frame_rate").value) <= 0.0:
+            raise ValueError("frame_rate must be positive")
+        if int(self.get_parameter("grab_timeout_ms").value) <= 0:
+            raise ValueError("grab_timeout_ms must be positive")
+        self.camera_info_path = resolve_camera_info_path(
+            str(self.get_parameter("camera_info_url").value)
+        )
+        self._calibration = self._load_calibration()
+
+        self.image_publisher = self.create_publisher(
+            Image, "image_raw", qos_profile_sensor_data
+        )
+        self.camera_info_publisher = self.create_publisher(
+            CameraInfo, "camera_info", qos_profile_sensor_data
+        )
+        self.create_service(
+            SetCameraInfo, "set_camera_info", self._set_camera_info_callback
+        )
+
+        try:
+            self._open_camera()
+            self._grab_thread = Thread(
+                target=self._grab_loop,
+                name="hikrobot_mvs_grab",
+                daemon=True,
+            )
+            self._grab_thread.start()
+        except Exception:
+            self.close()
+            raise
+
+    def _declare_parameters(self) -> None:
+        self.declare_parameter("serial_number", "")
+        self.declare_parameter("expected_model", "MV-CS020-10UM")
+        self.declare_parameter("camera_name", "hikrobot_front")
+        self.declare_parameter("frame_id", "camera_color_optical_frame")
+        self.declare_parameter("image_width", 1624)
+        self.declare_parameter("image_height", 1240)
+        self.declare_parameter("frame_rate", 30.0)
+        self.declare_parameter("exposure_time_us", 5000.0)
+        self.declare_parameter("gain_db", 0.0)
+        self.declare_parameter("auto_exposure", False)
+        self.declare_parameter("camera_info_url", "")
+        self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
+        self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
+        self.declare_parameter("grab_timeout_ms", 1000)
+
+    def _load_calibration(self) -> CameraCalibration | None:
+        if self.camera_info_path is None or not self.camera_info_path.exists():
+            target = self.camera_info_path or "an unset camera_info_url"
+            self.get_logger().warning(
+                f"No camera calibration found at {target}; publishing "
+                "uncalibrated CameraInfo. Trajectory calibration will remain "
+                "locked until valid intrinsics are installed."
+            )
+            return None
+        calibration = load_camera_calibration(self.camera_info_path)
+        self._validate_calibration_dimensions(calibration)
+        self.get_logger().info(
+            f"Loaded camera intrinsics from {self.camera_info_path}"
+        )
+        return calibration
+
+    def _validate_calibration_dimensions(
+        self, calibration: CameraCalibration
+    ) -> None:
+        if (
+            calibration.width != self.image_width
+            or calibration.height != self.image_height
+        ):
+            raise ValueError(
+                "camera calibration dimensions "
+                f"{calibration.width}x{calibration.height} do not match "
+                f"configured stream {self.image_width}x{self.image_height}"
+            )
+
+    def _import_mvs(self) -> Any:
+        python_path = Path(
+            str(self.get_parameter("mvs_python_path").value)
+        ).expanduser()
+        runtime_path = Path(
+            str(self.get_parameter("mvs_runtime_path").value)
+        ).expanduser()
+        if not python_path.is_dir():
+            raise RuntimeError(f"MVS Python bindings not found: {python_path}")
+        if not runtime_path.is_dir():
+            raise RuntimeError(f"MVS runtime not found: {runtime_path}")
+        os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
+        if str(python_path) not in sys.path:
+            sys.path.insert(0, str(python_path))
+        return importlib.import_module("MvCameraControl_class")
+
+    def _enumerate_usb_cameras(
+        self, mvs: Any
+    ) -> tuple[Any, list[DeviceDescriptor]]:
+        device_list = mvs.MV_CC_DEVICE_INFO_LIST()
+        result = mvs.MvCamera.MV_CC_EnumDevices(
+            mvs.MV_USB_DEVICE, device_list
+        )
+        self._require_ok("enumerate USB3 Vision devices", result)
+        descriptors: list[DeviceDescriptor] = []
+        for index in range(device_list.nDeviceNum):
+            device_info = cast(
+                device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
+            ).contents
+            usb_info = device_info.SpecialInfo.stUsb3VInfo
+            descriptors.append(
+                DeviceDescriptor(
+                    index=index,
+                    model=decode_c_string(usb_info.chModelName),
+                    serial=decode_c_string(usb_info.chSerialNumber),
+                    guid=decode_c_string(usb_info.chDeviceGUID),
+                )
+            )
+        return device_list, descriptors
+
+    def _open_camera(self) -> None:
+        mvs = self._import_mvs()
+        self._mvs = mvs
+        self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
+        self._sdk_initialized = True
+        device_list, descriptors = self._enumerate_usb_cameras(mvs)
+        selected = select_device(descriptors, self.selector, self.expected_model)
+        device_info = cast(
+            device_list.pDeviceInfo[selected.index],
+            POINTER(mvs.MV_CC_DEVICE_INFO),
+        ).contents
+
+        camera = mvs.MvCamera()
+        self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
+        self._camera = camera
+        self._handle_created = True
+        self._require_ok(
+            "open camera",
+            camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
+        )
+        self._camera_opened = True
+
+        self._set_required_enum("AcquisitionMode", "Continuous")
+        self._set_required_enum("TriggerMode", "Off")
+        self._set_required_enum("PixelFormat", "Mono8")
+        self._set_required_integer("OffsetX", 0)
+        self._set_required_integer("OffsetY", 0)
+        self._set_required_integer("Width", self.image_width)
+        self._set_required_integer("Height", self.image_height)
+        self._set_optional_bool("AcquisitionFrameRateEnable", True)
+        self._set_optional_float(
+            "AcquisitionFrameRate",
+            float(self.get_parameter("frame_rate").value),
+        )
+        if bool(self.get_parameter("auto_exposure").value):
+            self._set_required_enum("ExposureAuto", "Continuous")
+        else:
+            self._set_required_enum("ExposureAuto", "Off")
+            self._set_optional_float(
+                "ExposureTime",
+                float(self.get_parameter("exposure_time_us").value),
+            )
+            self._set_required_enum("GainAuto", "Off")
+            self._set_optional_float(
+                "Gain", float(self.get_parameter("gain_db").value)
+            )
+
+        self._require_ok(
+            "set latest-frame grab strategy",
+            camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
+        )
+        self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
+        self._grabbing = True
+        self.get_logger().info(
+            "Opened Hikrobot camera "
+            f"model={selected.model} serial={selected.serial} "
+            f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
+            "encoding=mono8"
+        )
+
+    def _set_required_enum(self, name: str, value: str) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetEnumValueByString(name, value),
+        )
+
+    def _set_required_integer(self, name: str, value: int) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetIntValueEx(name, value),
+        )
+
+    def _set_optional_bool(self, name: str, value: bool) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetBoolValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    def _set_optional_float(self, name: str, value: float) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetFloatValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    @staticmethod
+    def _require_ok(operation: str, result: int) -> None:
+        if result != 0:
+            hint = ""
+            if result == 0x80000203:
+                hint = (
+                    "; access denied: stop acquisition and close the MVS "
+                    "viewer or any other process using this camera"
+                )
+            raise RuntimeError(
+                f"MVS failed to {operation}: 0x{result:08x}{hint}"
+            )
+
+    def _grab_loop(self) -> None:
+        assert self._camera is not None
+        assert self._mvs is not None
+        timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
+        while not self._stop_event.is_set():
+            frame = self._mvs.MV_FRAME_OUT()
+            memset(byref(frame), 0, sizeof(frame))
+            result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
+            if result != 0:
+                self._timeout_count += 1
+                if self._timeout_count == 1 or self._timeout_count % 10 == 0:
+                    self.get_logger().warning(
+                        "MVS image acquisition timed out or failed: "
+                        f"0x{result:08x}; consecutive={self._timeout_count}"
+                    )
+                continue
+            try:
+                self._timeout_count = 0
+                self._publish_frame(frame)
+            except Exception as error:
+                self.get_logger().error(f"Failed to publish MVS frame: {error}")
+            finally:
+                free_result = self._camera.MV_CC_FreeImageBuffer(frame)
+                if free_result != 0:
+                    self.get_logger().error(
+                        f"MVS failed to release frame: 0x{free_result:08x}"
+                    )
+
+    def _publish_frame(self, frame: Any) -> None:
+        assert self._mvs is not None
+        info = frame.stFrameInfo
+        width = int(info.nWidth)
+        height = int(info.nHeight)
+        if width != self.image_width or height != self.image_height:
+            raise RuntimeError(
+                f"received {width}x{height}, expected "
+                f"{self.image_width}x{self.image_height}"
+            )
+        if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
+            raise RuntimeError(
+                f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
+            )
+        expected_size = width * height
+        if int(info.nFrameLen) < expected_size:
+            raise RuntimeError(
+                f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
+            )
+        stamp = self.get_clock().now().to_msg()
+        image = Image()
+        image.header.stamp = stamp
+        image.header.frame_id = self.frame_id
+        image.height = height
+        image.width = width
+        image.encoding = "mono8"
+        image.is_bigendian = 0
+        image.step = width
+        image.data = string_at(frame.pBufAddr, expected_size)
+
+        camera_info = self._make_camera_info(width, height)
+        camera_info.header.stamp = stamp
+        camera_info.header.frame_id = self.frame_id
+        self.image_publisher.publish(image)
+        self.camera_info_publisher.publish(camera_info)
+        self._frame_count += 1
+
+    def _make_camera_info(self, width: int, height: int) -> CameraInfo:
+        message = CameraInfo()
+        message.width = width
+        message.height = height
+        with self._camera_info_lock:
+            calibration = self._calibration
+        if calibration is None:
+            message.distortion_model = "plumb_bob"
+            message.d = []
+            message.k = [0.0] * 9
+            message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+            message.p = [0.0] * 12
+            return message
+        message.distortion_model = calibration.distortion_model
+        message.d = list(calibration.d)
+        message.k = list(calibration.k)
+        message.r = list(calibration.r)
+        message.p = list(calibration.p)
+        return message
+
+    def _set_camera_info_callback(
+        self,
+        request: SetCameraInfo.Request,
+        response: SetCameraInfo.Response,
+    ) -> SetCameraInfo.Response:
+        try:
+            if self.camera_info_path is None:
+                raise ValueError(
+                    "camera_info_url is empty; set it before saving calibration"
+                )
+            message = request.camera_info
+            if (
+                int(message.width) != self.image_width
+                or int(message.height) != self.image_height
+            ):
+                raise ValueError(
+                    f"calibration is {message.width}x{message.height}, expected "
+                    f"{self.image_width}x{self.image_height}"
+                )
+            mapping = calibration_to_mapping(self.camera_name, message)
+            self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
+            temporary_path = self.camera_info_path.with_suffix(
+                self.camera_info_path.suffix + ".tmp"
+            )
+            temporary_path.write_text(
+                yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
+            )
+            temporary_path.replace(self.camera_info_path)
+            calibration = load_camera_calibration(self.camera_info_path)
+            self._validate_calibration_dimensions(calibration)
+            with self._camera_info_lock:
+                self._calibration = calibration
+            response.success = True
+            response.status_message = (
+                f"saved camera calibration to {self.camera_info_path}"
+            )
+            self.get_logger().info(response.status_message)
+        except Exception as error:
+            response.success = False
+            response.status_message = str(error)
+            self.get_logger().error(
+                f"Rejected camera calibration: {response.status_message}"
+            )
+        return response
+
+    def close(self) -> None:
+        if self._closed:
+            return
+        self._closed = True
+        self._stop_event.set()
+        if self._grab_thread is not None:
+            self._grab_thread.join(timeout=2.0)
+        if self._camera is not None and self._camera_opened:
+            if self._grabbing:
+                result = self._camera.MV_CC_StopGrabbing()
+                if result != 0:
+                    self.get_logger().warning(
+                        f"MVS failed to stop grabbing: 0x{result:08x}"
+                    )
+                self._grabbing = False
+            result = self._camera.MV_CC_CloseDevice()
+            if result != 0:
+                self.get_logger().warning(
+                    f"MVS failed to close camera: 0x{result:08x}"
+                )
+            self._camera.MV_CC_DestroyHandle()
+        elif self._camera is not None and self._handle_created:
+            self._camera.MV_CC_DestroyHandle()
+        self._camera = None
+        self._handle_created = False
+        self._camera_opened = False
+        if self._sdk_initialized and self._mvs is not None:
+            self._mvs.MvCamera.MV_CC_Finalize()
+            self._sdk_initialized = False
+
+    def destroy_node(self) -> None:
+        self.close()
+        super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: HikrobotCameraNode | None = None
+    try:
+        node = HikrobotCameraNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -0,0 +1,860 @@
+"""Safely replay a complete three-camera session without moving the hand."""
+
+from __future__ import annotations
+
+import argparse
+from collections import defaultdict
+from dataclasses import replace
+import hashlib
+import json
+import math
+import os
+from pathlib import Path
+import re
+import tempfile
+from typing import Any, Mapping, Sequence
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+import yaml
+
+from .extrinsics import load_three_camera_extrinsics
+from .full_hand import (
+    HandCalibrationProfile,
+    JointCurveFit,
+    build_calibration_motion_command,
+    build_compact_payload,
+    get_hand_calibration_profile,
+    validate_compact_payload,
+)
+from .storage import atomic_write_json
+from .urdf_zero import (
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    fit_joint_axis_measurement,
+    fit_rotation_joint_curve,
+    get_zero_calibration_profile,
+    rotation_curve_holdout_errors,
+    solve_urdf_zero_offsets,
+    write_zero_corrected_urdf,
+)
+
+
+def _sha256(path: Path) -> str:
+    digest = hashlib.sha256()
+    with path.open("rb") as stream:
+        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _output_suffix(output_tag: str | None) -> str:
+    """Return a filename-safe suffix for a non-destructive replay variant."""
+    if output_tag is None:
+        return ""
+    tag = str(output_tag)
+    if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
+        raise ValueError(
+            "output tag must be 1-64 filename-safe characters, beginning "
+            "with a letter or digit"
+        )
+    return f"_{tag}"
+
+
+def _load_parameters(path: Path) -> dict[str, Any]:
+    payload = yaml.safe_load(path.read_text(encoding="utf-8"))
+    if not isinstance(payload, Mapping):
+        raise ValueError("calibration config must contain a mapping")
+    node = payload.get("/**", payload.get("g20_calibration"))
+    if not isinstance(node, Mapping) or not isinstance(
+        node.get("ros__parameters"), Mapping
+    ):
+        raise ValueError("calibration config is missing ros__parameters")
+    return dict(node["ros__parameters"])
+
+
+def _latest_attempt_records(
+    rows: Sequence[Mapping[str, Any]],
+) -> dict[str, list[dict[str, Any]]]:
+    """Reproduce the online retry buffer from append-only raw samples.
+
+    Online retry clears only the failed joint/cycle/direction from memory,
+    while JSONL deliberately retains every attempt for audit.  Offline replay
+    must therefore select the greatest attempt independently for each logical
+    trajectory rather than mixing rejected attempts into the final fit.
+    """
+    samples = [dict(row) for row in rows if row.get("kind") == "sample"]
+    latest_attempt: dict[tuple[str, int, str], int] = {}
+    for row in samples:
+        key = (
+            str(row["joint"]),
+            int(row["cycle"]),
+            str(row["direction"]),
+        )
+        latest_attempt[key] = max(
+            latest_attempt.get(key, 0), int(row.get("attempt", 1))
+        )
+    records: dict[str, list[dict[str, Any]]] = defaultdict(list)
+    for row in samples:
+        key = (
+            str(row["joint"]),
+            int(row["cycle"]),
+            str(row["direction"]),
+        )
+        if int(row.get("attempt", 1)) == latest_attempt[key]:
+            records[key[0]].append(row)
+    return dict(records)
+
+
+def _load_raw_session(
+    session_dir: Path,
+) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
+    raw_path = session_dir / "raw_samples.jsonl"
+    if not raw_path.is_file():
+        raise ValueError(f"raw session does not exist: {raw_path}")
+    rows = [
+        json.loads(line)
+        for line in raw_path.read_text(encoding="utf-8").splitlines()
+        if line.strip()
+    ]
+    starts = [row for row in rows if row.get("kind") == "session_start"]
+    if len(starts) != 1:
+        raise ValueError("raw session must contain exactly one session_start")
+    return starts[0], _latest_attempt_records(rows), raw_path
+
+
+def _fit_curve(
+    name: str,
+    records: Sequence[Mapping[str, Any]],
+    *,
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+) -> JointCurveFit:
+    motor = profile.joint_specs[name].motor_index
+    return fit_rotation_joint_curve(
+        records,
+        zero_command_u8=int(baseline[motor]),
+        command_increasing=profile.joint_specs[name].command_increasing,
+    )
+
+
+def _fit_axes(
+    *,
+    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+    extrinsics_file: Path,
+    repetitions: int,
+) -> list[JointAxisMeasurement]:
+    zero_profile = get_zero_calibration_profile(profile.side, profile.model)
+    extrinsics = load_three_camera_extrinsics(extrinsics_file)
+    cache: dict[tuple[str, int], JointAxisMeasurement] = {}
+    upstream_by_joint = zero_profile.parallel_axis_parent_joint
+
+    def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
+        key = (name, cycle)
+        if key in cache:
+            return cache[key]
+        upstream = upstream_by_joint.get(name)
+        constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
+        spec = profile.joint_specs[name]
+        view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
+            [0.0, 0.0, 1.0], dtype=float
+        )
+        result = fit_joint_axis_measurement(
+            name,
+            records_by_joint[name],
+            cycle=cycle,
+            zero_command_u8=int(baseline[spec.motor_index]),
+            axis_common_constraint=constraint,
+            constrained_circle_joints=zero_profile.constrained_circle_joints,
+            view_normal_common_xyz=view_normal,
+            command_increasing=spec.command_increasing,
+        )
+        condition = build_calibration_motion_command(
+            spec,
+            int(baseline[spec.motor_index]),
+            baseline=baseline,
+            profile=profile,
+        )
+        result = replace(
+            result,
+            condition_command_u8=tuple(float(value) for value in condition),
+            view_normal_common_xyz=tuple(float(value) for value in view_normal),
+        )
+        cache[key] = result
+        return result
+
+    return [
+        fit_one(name, cycle)
+        for name in zero_profile.axis_joints
+        for cycle in range(repetitions)
+    ]
+
+
+def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
+    maximum = 0.0
+    for left in axes:
+        for right in axes:
+            maximum = max(
+                maximum,
+                math.acos(
+                    abs(
+                        float(
+                            np.clip(
+                                np.asarray(left, dtype=float)
+                                @ np.asarray(right, dtype=float),
+                                -1.0,
+                                1.0,
+                            )
+                        )
+                    )
+                ),
+            )
+    return maximum
+
+
+def _quality_failures(
+    *,
+    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
+    profile: HandCalibrationProfile,
+    baseline: Sequence[int],
+    fits: Mapping[str, JointCurveFit],
+    axes: Sequence[JointAxisMeasurement],
+    parameters: Mapping[str, Any],
+) -> list[str]:
+    failures: list[str] = []
+    repetitions = int(parameters["repetitions"])
+    zero_profile = get_zero_calibration_profile(profile.side, profile.model)
+    axis_by_key = {(item.joint, item.cycle): item for item in axes}
+    expected_directions = {"decreasing", "increasing"}
+    for name in profile.measured_joints:
+        records = list(records_by_joint.get(name, ()))
+        if not records:
+            failures.append(f"{name}: no samples")
+            continue
+        attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
+        for record in records:
+            attempts_by_direction[
+                (int(record["cycle"]), str(record["direction"]))
+            ].add(int(record.get("attempt", 1)))
+        for cycle in range(repetitions):
+            for direction in expected_directions:
+                selected = [
+                    record
+                    for record in records
+                    if int(record["cycle"]) == cycle
+                    and str(record["direction"]) == direction
+                ]
+                attempts = attempts_by_direction.get((cycle, direction), set())
+                if len(attempts) != 1:
+                    attempt_list = sorted(attempts)
+                    failures.append(
+                        f"{name} cycle {cycle + 1} {direction}: "
+                        f"ambiguous attempts {attempt_list}"
+                    )
+                    continue
+                commands = sorted({int(record["command_u8"]) for record in selected})
+                if len(selected) < int(parameters["minimum_sweep_frames"]):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
+                if not commands or max(commands) - min(commands) < float(
+                    parameters["minimum_state_span_u8"]
+                ):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
+                if len(commands) < int(parameters["minimum_sweep_bins"]):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
+                if 0 not in commands or 255 not in commands:
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
+                if commands and max(np.diff(commands), default=0) > int(
+                    parameters["maximum_bin_gap"]
+                ):
+                    failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
+
+        sync_p95 = float(
+            np.percentile(
+                [float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
+                95.0,
+            )
+        )
+        if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
+            failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
+        spec = profile.joint_specs[name]
+        fit = fits[name]
+        orthogonal_limit = math.radians(
+            float(
+                parameters[
+                    "active_maximum_rotation_orthogonal_rms_deg"
+                    if spec.active
+                    else "passive_maximum_rotation_orthogonal_rms_deg"
+                ]
+            )
+        )
+        if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
+            failures.append(f"{name}: rotation orthogonal RMS")
+        if float(fit.quality["arc_rad"]) < math.radians(
+            float(parameters["trajectory_minimum_arc_deg"])
+        ):
+            failures.append(f"{name}: trajectory arc")
+        monotonic_limit = math.radians(
+            float(
+                parameters[
+                    "maximum_monotonic_correction_deg"
+                    if spec.active
+                    else "passive_maximum_monotonic_correction_deg"
+                ]
+            )
+        )
+        hysteresis_limit = math.radians(
+            float(
+                parameters[
+                    "maximum_hysteresis_deg"
+                    if spec.active
+                    else "passive_maximum_hysteresis_deg"
+                ]
+            )
+        )
+        if fit.maximum_monotonic_correction_rad > monotonic_limit:
+            failures.append(f"{name}: monotonic correction")
+        if fit.maximum_hysteresis_rad > hysteresis_limit:
+            failures.append(f"{name}: hysteresis")
+
+        cycle_travels: list[float] = []
+        cycle_axes: list[Sequence[float]] = []
+        cycle_axis_sources: list[str] = []
+        for cycle in range(repetitions):
+            cycle_fit = _fit_curve(
+                name,
+                [record for record in records if int(record["cycle"]) == cycle],
+                profile=profile,
+                baseline=baseline,
+            )
+            cycle_travels.append(
+                abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
+            )
+            axis = axis_by_key[(name, cycle)]
+            cycle_axes.append(axis.axis_common_xyz)
+            cycle_axis_sources.append(axis.axis_direction_source)
+            if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
+                failures.append(f"{name} cycle {cycle + 1}: radial RMS")
+            if axis.pose_axis_line_rms_m > float(
+                parameters["axis_maximum_pose_line_rms_m"]
+            ):
+                failures.append(
+                    f"{name} cycle {cycle + 1}: pose axis-line RMS"
+                )
+            if name not in zero_profile.constrained_circle_joints:
+                plane_limit = float(
+                    parameters[
+                        "axis_maximum_plane_rms_m"
+                        if spec.active
+                        else "passive_axis_maximum_plane_rms_m"
+                    ]
+                )
+                if axis.plane_rms_m > plane_limit:
+                    failures.append(f"{name} cycle {cycle + 1}: plane RMS")
+                if (
+                    name not in zero_profile.constrained_circle_joints
+                    and axis.rotation_circle_axis_difference_rad > math.radians(
+                        float(parameters["axis_maximum_rotation_circle_difference_deg"])
+                    )
+                ):
+                    failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
+        travel_limit = math.radians(
+            float(
+                parameters[
+                    "trajectory_maximum_cycle_travel_difference_deg"
+                    if spec.active
+                    else "passive_maximum_cycle_travel_difference_deg"
+                ]
+            )
+        )
+        if max(cycle_travels) - min(cycle_travels) > travel_limit:
+            failures.append(f"{name}: cycle travel difference")
+        if (
+            not all(
+                source == "upstream_constraint"
+                for source in cycle_axis_sources
+            )
+            and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
+                float(parameters["zero_maximum_axis_cycle_difference_deg"])
+            )
+        ):
+            failures.append(f"{name}: cycle axis difference")
+    return failures
+
+
+def _joint_xml(path: Path) -> dict[str, ET.Element]:
+    return {
+        str(joint.get("name")): joint
+        for joint in ET.parse(path).getroot().findall("joint")
+    }
+
+
+def _triplet(value: str) -> np.ndarray:
+    return np.asarray([float(item) for item in value.split()], dtype=float)
+
+
+def _validate_corrected_urdf(
+    *,
+    source: Path,
+    corrected: Path,
+    offsets: Mapping[str, float],
+    axes: Sequence[JointAxisMeasurement],
+    curves: Mapping[str, JointCurveFit],
+    motor_by_joint: Mapping[str, int],
+    inherited_zero_joints: Mapping[str, str],
+) -> dict[str, float]:
+    source_joints = _joint_xml(source)
+    corrected_joints = _joint_xml(corrected)
+    if set(source_joints) != set(corrected_joints):
+        raise ValueError("corrected URDF changed the joint set")
+    maximum_origin_rotation_error = 0.0
+    maximum_origin_translation_error = 0.0
+    for name, original_joint in source_joints.items():
+        corrected_joint = corrected_joints[name]
+        original_origin = original_joint.find("origin")
+        corrected_origin = corrected_joint.find("origin")
+        if original_origin is None or corrected_origin is None:
+            continue
+        original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
+        corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
+        maximum_origin_translation_error = max(
+            maximum_origin_translation_error,
+            float(np.linalg.norm(corrected_xyz - original_xyz)),
+        )
+        original_rotation = Rotation.from_euler(
+            "xyz", _triplet(original_origin.get("rpy", "0 0 0"))
+        )
+        corrected_rotation = Rotation.from_euler(
+            "xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
+        )
+        expected = original_rotation
+        if name in offsets:
+            axis_node = original_joint.find("axis")
+            axis = _triplet(
+                "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
+            )
+            axis /= np.linalg.norm(axis)
+            expected = original_rotation * Rotation.from_rotvec(
+                axis * float(offsets[name])
+            )
+        error = float((expected.inv() * corrected_rotation).magnitude())
+        maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
+        original_limit = original_joint.find("limit")
+        corrected_limit = corrected_joint.find("limit")
+        if original_limit is not None and corrected_limit is not None:
+            if (
+                original_limit.get("lower") != corrected_limit.get("lower")
+                or original_limit.get("upper") != corrected_limit.get("upper")
+            ):
+                raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
+    if maximum_origin_translation_error > 1.0e-12:
+        raise ValueError("corrected URDF changed a joint origin translation")
+    if maximum_origin_rotation_error > 1.0e-10:
+        raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
+    original_model = UrdfKinematicModel(source)
+    corrected_model = UrdfKinematicModel(corrected)
+    maximum_axis_error = 0.0
+    maximum_point_error = 0.0
+    for measurement in axes:
+        state = (
+            measurement.condition_state_u8
+            if measurement.condition_command_u8 is None
+            else measurement.condition_command_u8
+        )
+        angles = _angles_from_state(
+            state,
+            curves=curves,
+            motor_by_joint=motor_by_joint,
+            inherited_zero_joints=inherited_zero_joints,
+        )
+        expected_axis, expected_point = original_model.axis_line(
+            measurement.joint,
+            zero_offsets=offsets,
+            joint_angles=angles,
+        )
+        actual_axis, actual_point = corrected_model.axis_line(
+            measurement.joint,
+            zero_offsets={},
+            joint_angles=angles,
+        )
+        maximum_axis_error = max(
+            maximum_axis_error,
+            math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
+        )
+        maximum_point_error = max(
+            maximum_point_error,
+            float(np.linalg.norm(expected_point - actual_point)),
+        )
+    if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
+        raise ValueError("written URDF kinematics differ from the solved correction")
+    return {
+        "maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
+        "maximum_origin_translation_error_m": maximum_origin_translation_error,
+        "maximum_axis_equivalence_error_rad": maximum_axis_error,
+        "maximum_axis_point_equivalence_error_m": maximum_point_error,
+    }
+
+
+def replay_session(
+    session_dir: str | Path,
+    *,
+    serial_number: str | None = None,
+    config_file: str | Path | None = None,
+    write_outputs: bool = False,
+    output_tag: str | None = None,
+) -> dict[str, Any]:
+    session = Path(session_dir).expanduser().resolve()
+    package_root = Path(__file__).resolve().parents[1]
+    config = (
+        package_root / "config" / "three_camera_calibration.yaml"
+        if config_file is None
+        else Path(config_file).expanduser().resolve()
+    )
+    parameters = _load_parameters(config)
+    start, records_by_joint, raw_path = _load_raw_session(session)
+    hand_model = str(start.get("hand_model", "G20")).upper()
+    side = str(start["hand_type"]).lower()
+    profile = get_hand_calibration_profile(side, hand_model)
+    zero_profile = get_zero_calibration_profile(side, hand_model)
+    baseline = tuple(int(value) for value in start["baseline_command_u8"])
+    if len(baseline) != 20:
+        raise ValueError("session baseline must contain exactly 20 commands")
+    if set(records_by_joint) != set(profile.measured_joints):
+        raise ValueError("raw session does not contain exactly the measured joint set")
+    source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
+    extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
+    if not source_urdf.is_file() or not extrinsics_file.is_file():
+        raise ValueError("session source URDF or camera extrinsics is missing")
+    if "zero_calibrated" in source_urdf.stem.lower():
+        raise ValueError("offline replay requires the original CAD URDF")
+    hand_serial = str(serial_number or session.parent.name)
+    output_suffix = _output_suffix(output_tag)
+    repetitions = int(parameters["repetitions"])
+
+    measured_fits = {
+        name: _fit_curve(
+            name,
+            records_by_joint[name],
+            profile=profile,
+            baseline=baseline,
+        )
+        for name in profile.measured_joints
+    }
+    training_fits = {
+        name: _fit_curve(
+            name,
+            [
+                record
+                for record in records_by_joint[name]
+                if int(record["cycle"]) in {0, 1}
+            ],
+            profile=profile,
+            baseline=baseline,
+        )
+        for name in profile.measured_joints
+    }
+    axes = _fit_axes(
+        records_by_joint=records_by_joint,
+        profile=profile,
+        baseline=baseline,
+        extrinsics_file=extrinsics_file,
+        repetitions=repetitions,
+    )
+    failures = _quality_failures(
+        records_by_joint=records_by_joint,
+        profile=profile,
+        baseline=baseline,
+        fits=measured_fits,
+        axes=axes,
+        parameters=parameters,
+    )
+    if failures:
+        raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
+
+    holdout_by_joint = {
+        name: rotation_curve_holdout_errors(
+            training_fits[name],
+            [
+                record
+                for record in records_by_joint[name]
+                if int(record["cycle"]) == 2
+            ],
+            zero_command_u8=int(
+                baseline[profile.joint_specs[name].motor_index]
+            ),
+        )
+        for name in profile.measured_joints
+    }
+    trajectory_errors = np.abs(
+        np.asarray(
+            [value for values in holdout_by_joint.values() for value in values],
+            dtype=float,
+        )
+    )
+    maximum_validation_mae = math.radians(
+        float(parameters["maximum_validation_mae_deg"])
+    )
+    maximum_validation_p95 = math.radians(
+        float(parameters["maximum_validation_p95_deg"])
+    )
+    trajectory_mae = float(np.mean(trajectory_errors))
+    trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
+    if (
+        trajectory_mae > maximum_validation_mae
+        or trajectory_p95 > maximum_validation_p95
+    ):
+        raise ValueError("third-cycle trajectory holdout failed")
+
+    motor_by_joint = {
+        name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
+    }
+    joint_limits: dict[str, float] = {}
+    solve_arguments = {
+        "source_urdf": source_urdf,
+        "measurements": axes,
+        "motor_by_joint": motor_by_joint,
+        "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
+        "finger_maximum_offset_rad": math.radians(
+            float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
+        ),
+        "joint_maximum_offset_rad": joint_limits,
+        "maximum_cycle_difference_rad": math.radians(
+            float(parameters["zero_maximum_axis_cycle_difference_deg"])
+        ),
+        "maximum_axis_cone_mismatch_rad": math.radians(
+            float(parameters["zero_maximum_axis_cone_mismatch_deg"])
+        ),
+        "maximum_pose_axis_line_rms_m": float(
+            parameters["axis_maximum_pose_line_rms_m"]
+        ),
+        "maximum_validation_mae_rad": maximum_validation_mae,
+        "maximum_validation_p95_rad": maximum_validation_p95,
+        "hand_type": side,
+        "hand_model": hand_model,
+    }
+    holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
+    if not holdout_zero.passed:
+        failure = {
+            "reasons": dict(holdout_zero.failure_reasons),
+            "fitted_offsets_deg": {
+                name: math.degrees(value)
+                for name, value in holdout_zero.direct_offsets_rad.items()
+            },
+            "cycle_offsets_deg": {
+                name: [math.degrees(value) for value in values]
+                for name, values in holdout_zero.cycle_offsets_rad.items()
+            },
+        }
+        raise ValueError(
+            "third-cycle zero/URDF holdout failed: "
+            + json.dumps(failure, ensure_ascii=False, sort_keys=True)
+        )
+    final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
+    if not final_zero.passed:
+        raise ValueError(
+            "all-cycle zero refit failed: "
+            + json.dumps(
+                {
+                    "reasons": dict(final_zero.failure_reasons),
+                    "fitted_offsets_deg": {
+                        name: math.degrees(value)
+                        for name, value in final_zero.direct_offsets_rad.items()
+                    },
+                },
+                ensure_ascii=False,
+                sort_keys=True,
+            )
+        )
+    for target, source_name in zero_profile.inherited_static_zero_joints.items():
+        if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
+            raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
+    for target in (
+        set(zero_profile.inherited_zero_joints)
+        - set(zero_profile.inherited_static_zero_joints)
+    ):
+        if final_zero.all_active_offsets_rad[target] != 0.0:
+            raise ValueError(f"unobserved static zero must retain source CAD: {target}")
+
+    validation_errors = [
+        float(value) for values in holdout_by_joint.values() for value in values
+    ]
+    validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
+    payload = build_compact_payload(
+        serial_number=hand_serial,
+        measured_fits=measured_fits,
+        urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
+        validation_errors_rad=validation_errors,
+        passed=True,
+        baseline=baseline,
+        side=side,
+        model=hand_model,
+    )
+    validate_compact_payload(payload)
+
+    stamp = session.name
+    final_json = session / (
+        f"{hand_model.lower()}_{side}_{hand_serial}_calibration"
+        f"{output_suffix}.json"
+    )
+    expected_urdf_name = (
+        f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
+        f"{output_suffix}.urdf"
+    )
+    final_urdf = source_urdf.parent / expected_urdf_name
+    report_path = session / (
+        f"{hand_model.lower()}_{side}_{hand_serial}_offline_validation"
+        f"{output_suffix}.json"
+    )
+    if write_outputs:
+        existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
+        if existing:
+            raise ValueError(
+                "refusing to overwrite replay outputs: "
+                + ", ".join(str(path) for path in existing)
+            )
+
+    source_hash_before = _sha256(source_urdf)
+    with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
+        candidate = write_zero_corrected_urdf(
+            source_urdf=source_urdf,
+            output_directory=temporary,
+            serial_number=hand_serial,
+            offsets_rad=final_zero.all_active_offsets_rad,
+            timestamp=stamp,
+        )
+        urdf_checks = _validate_corrected_urdf(
+            source=source_urdf,
+            corrected=candidate,
+            offsets=final_zero.all_active_offsets_rad,
+            axes=axes,
+            curves=measured_fits,
+            motor_by_joint=motor_by_joint,
+            inherited_zero_joints=zero_profile.inherited_zero_joints,
+        )
+        residual_zero = solve_urdf_zero_offsets(
+            curves=measured_fits,
+            fixed_direct_zero_offsets_rad={
+                name: 0.0
+                for name in zero_profile.fixed_direct_zero_offsets_rad
+            },
+            static_output_zero_offsets_rad={
+                name: 0.0
+                for name in zero_profile.static_output_zero_offsets_rad
+            },
+            **{**solve_arguments, "source_urdf": candidate},
+        )
+        maximum_residual_offset = max(
+            abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
+        )
+        if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
+            raise ValueError("written URDF retains a significant zero correction")
+        candidate_hash = _sha256(candidate)
+        if write_outputs:
+            os.replace(candidate, final_urdf)
+
+    if _sha256(source_urdf) != source_hash_before:
+        raise ValueError("source URDF changed during offline replay")
+
+    report: dict[str, Any] = {
+        "passed": True,
+        "session_dir": str(session),
+        "model": hand_model,
+        "side": side,
+        "serial_number": hand_serial,
+        "output_tag": output_tag,
+        "raw_samples_sha256": _sha256(raw_path),
+        "source_urdf": str(source_urdf),
+        "source_urdf_sha256": source_hash_before,
+        "joint_limits_deg": {
+            "finger_default": float(
+                parameters.get("zero_finger_maximum_offset_deg", 3.0)
+            ),
+            "thumb_default": float(parameters["zero_maximum_offset_deg"]),
+        },
+        "static_zero_policy": "direct_measurements_only",
+        "fixed_zero_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
+        },
+        "static_output_zero_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in zero_profile.static_output_zero_offsets_rad.items()
+        },
+        "direct_offsets_deg": {
+            name: math.degrees(value)
+            for name, value in final_zero.direct_offsets_rad.items()
+        },
+        "cycle_offsets_deg": {
+            name: [math.degrees(value) for value in values]
+            for name, values in holdout_zero.cycle_offsets_rad.items()
+        },
+        "offset_uncertainty_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.offset_uncertainty_rad.items()
+        },
+        "trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
+        "trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
+        "zero_holdout_error_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.validation_error_by_joint_rad.items()
+        },
+        "zero_original_error_deg": {
+            name: math.degrees(value)
+            for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
+        },
+        "zero_improvement_95pct_lower_deg": {
+            name: math.degrees(value)
+            for name, value in (
+                holdout_zero.validation_improvement_confidence_lower_rad.items()
+            )
+        },
+        "corrected_urdf_checks": {
+            **urdf_checks,
+            "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
+        },
+        "corrected_urdf_sha256": candidate_hash,
+        "final_json": str(final_json) if write_outputs else None,
+        "corrected_urdf": str(final_urdf) if write_outputs else None,
+    }
+    if write_outputs:
+        atomic_write_json(final_json, payload)
+        report["final_json_sha256"] = _sha256(final_json)
+        if _sha256(final_urdf) != candidate_hash:
+            raise ValueError("formal corrected URDF differs from validated candidate")
+        atomic_write_json(report_path, report)
+        report["validation_report"] = str(report_path)
+    return report
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description=(
+            "Replay and independently validate a complete supported-hand "
+            "calibration session."
+        )
+    )
+    parser.add_argument("session_dir")
+    parser.add_argument("--serial-number", default=None)
+    parser.add_argument("--config-file", default=None)
+    parser.add_argument("--write", action="store_true")
+    parser.add_argument(
+        "--output-tag",
+        default=None,
+        help="safe suffix for a replay variant; existing outputs are never overwritten",
+    )
+    arguments = parser.parse_args()
+    result = replay_session(
+        arguments.session_dir,
+        serial_number=arguments.serial_number,
+        config_file=arguments.config_file,
+        write_outputs=arguments.write,
+        output_tag=arguments.output_tag,
+    )
+    print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
+
+
+if __name__ == "__main__":
+    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py

@@ -0,0 +1,105 @@
+"""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 append_jsonl_many(
+    path: str | Path, payloads: Iterable[Mapping[str, Any]]
+) -> None:
+    """Durably append a batch while paying the fsync cost only once."""
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    lines = [
+        json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
+        for payload in payloads
+    ]
+    if not lines:
+        return
+    with destination.open("a", encoding="utf-8") as stream:
+        stream.write("\n".join(lines) + "\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/three_camera_diagnostics.py

@@ -0,0 +1,504 @@
+"""Chinese, operator-facing diagnostics for three-camera calibration."""
+
+from __future__ import annotations
+
+import re
+from typing import Any, Mapping
+
+
+STATE_NAMES_ZH = {
+    "PREFLIGHT": "设备和标签预检",
+    "WAIT_START": "等待开始标定",
+    "RETURN_BASELINE": "正在恢复目标姿态",
+    "PREPARE_SWEEP": "正在到达扫描起点",
+    "SWEEP": "正在采集轨迹",
+    "FITTING": "正在拟合轨迹和零位",
+    "VALIDATION_MOVE": "正在移动到随机复测位置",
+    "VALIDATION_CAPTURE": "正在采集随机复测数据",
+    "PAUSED": "标定已暂停",
+    "ABORTED": "标定已终止",
+    "COMPLETE": "标定已完成",
+}
+
+VIEW_NAMES_ZH = {
+    "front": "正面",
+    "side": "侧面",
+    "top": "上面",
+}
+
+JOINT_NAMES_ZH = {
+    "thumb_cmc_pitch": "拇指CMC俯仰",
+    "thumb_cmc_roll": "拇指CMC滚转",
+    "thumb_mcp": "拇指MCP",
+    "thumb_ip": "拇指IP（被动）",
+    "index_mcp_roll": "食指MCP侧摆",
+    "index_mcp_pitch": "食指MCP屈伸",
+    "index_pip": "食指PIP",
+    "index_dip": "食指DIP（被动）",
+    "middle_mcp_roll": "中指MCP侧摆",
+    "middle_mcp_pitch": "中指MCP屈伸",
+    "middle_pip": "中指PIP",
+    "middle_dip": "中指DIP（被动）",
+    "ring_mcp_roll": "无名指MCP侧摆",
+    "ring_mcp_pitch": "无名指MCP屈伸",
+    "ring_pip": "无名指PIP",
+    "ring_dip": "无名指DIP（被动）",
+    "pinky_mcp_roll": "小指MCP侧摆",
+    "pinky_mcp_pitch": "小指MCP屈伸",
+    "pinky_pip": "小指PIP",
+    "pinky_dip": "小指DIP（被动）",
+    "thumb_cmc_yaw": "拇指CMC侧摆",
+}
+
+
+def _format_u8(value: Any) -> str:
+    if value is None:
+        return "尚无反馈"
+    return f"{float(value):.1f}"
+
+
+def _task_text(active: Mapping[str, Any]) -> str:
+    if not active:
+        return "尚无活动任务"
+    view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
+    if active.get("kind") == "fit_failure":
+        joints = active.get("joints", [])
+        joint_text = "/".join(
+            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+        )
+        return (
+            f"{view}机位，{joint_text}拟合检查失败，"
+            f"电机{active.get('motor_index')}，"
+            f"第{active.get('attempt', 1)}次尝试"
+        )
+    if active.get("kind") == "zero_model_failure":
+        joints = active.get("joints", [])
+        joint_text = "/".join(
+            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+        )
+        return (
+            f"{view}机位，{joint_text}零位/URDF验证失败，"
+            f"电机{active.get('motor_index')}，不会自动重扫"
+        )
+    if active.get("kind") == "motion_stall":
+        return (
+            f"电机{active.get('motor_index', '?')}运动停滞，目标"
+            f"{_format_u8(active.get('target_u8'))}、实际"
+            f"{_format_u8(active.get('actual_u8'))}"
+        )
+    if active.get("kind") == "validation":
+        return (
+            f"{view}机位，随机复测，电机{active.get('motor_index')}，"
+            f"目标命令{active.get('command_u8')}"
+        )
+    joints = active.get("joints", [])
+    joint_text = "/".join(
+        JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
+    )
+    start = active.get("start_u8")
+    target = active.get("target_u8")
+    cycle = active.get("cycle", "?")
+    repetitions = active.get("repetitions", "?")
+    direction_index = active.get("direction_index")
+    direction_text = (
+        "" if direction_index is None else f"第{direction_index}/2程，"
+    )
+    task = (
+        f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
+        f"第{cycle}/{repetitions}轮，{direction_text}{start}→{target}"
+    )
+    sequence = active.get("cycle_sequence_u8", [])
+    if len(sequence) == 3:
+        task += "（本轮" + "→".join(str(value) for value in sequence) + "）"
+    fit_attempt = int(active.get("fit_attempt", 1))
+    if fit_attempt > 1:
+        task += (
+            f"（整关节自动重采第{fit_attempt}/"
+            f"{active.get('fit_attempt_limit', '?')}次）"
+        )
+    return task
+
+
+def three_camera_reason_zh(
+    state: str,
+    reason: str,
+    active: Mapping[str, Any],
+) -> tuple[str, str]:
+    """Translate a reason code and provide one concrete operator action."""
+    reason = str(reason)
+    sample = active.get("sample", {}) if active else {}
+    missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
+    sample_range = (
+        f"{_format_u8(sample.get('minimum_u8'))}～"
+        f"{_format_u8(sample.get('maximum_u8'))}"
+    )
+    tolerance = sample.get("endpoint_tolerance_u8", "?")
+
+    if reason.startswith("motor_state_stalled:"):
+        fields = reason.split(":")
+        context = fields[1] if len(fields) > 1 else "unknown"
+        error_match = re.search(r"error_u8=([0-9.]+)", reason)
+        error = error_match.group(1) if error_match else "未知"
+        motor = active.get("motor_index")
+        if motor is not None:
+            return (
+                f"电机{motor}反馈连续8秒没有向目标推进；目标"
+                f"{_format_u8(active.get('target_u8'))}、实际"
+                f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
+                f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
+                f"（阶段={context}）。程序已保持当前位置。",
+                "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
+                "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
+            )
+        return (
+            f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
+            f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
+            "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
+            "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
+        )
+
+    if "URDF zero offset reached the configured" in reason:
+        bound_match = re.search(
+            r"configured\s+([0-9.]+)\s+degree bound", reason
+        )
+        bound = bound_match.group(1) if bound_match else "配置的"
+        hit_text = ""
+        if "bound:" in reason:
+            hit_text = reason.split("bound:", 1)[1].split(
+                "; all_offsets:", 1
+            )[0]
+            for name, label in JOINT_NAMES_ZH.items():
+                hit_text = hit_text.replace(name, label)
+        hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
+        return (
+            f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
+            "零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
+            "不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
+            "内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
+            "产生的轨迹不能直接生成修正URDF。",
+        )
+
+    if reason == "sweep_missing_endpoint_bin":
+        missing_text = "、".join(str(value) for value in missing) or "0或255"
+        return (
+            f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
+            f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
+            f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
+            "端点，但该时刻没有同时取得有效Tag图像和电机状态。",
+            "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
+            "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
+        )
+    if reason == "sweep_bins_too_few":
+        return (
+            f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
+            f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
+            "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
+        )
+    if reason == "sweep_bin_gap_too_large":
+        return (
+            f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
+            f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
+            "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
+        )
+    if reason == "synchronised_tag_state_timeout":
+        return (
+            "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
+            "按时间戳配对”的图像帧。",
+            "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
+            "程序会重扫当前方向。",
+        )
+    if reason == "sweep_start_position_timeout":
+        return (
+            f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
+            f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
+            "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "sweep_timeout":
+        return (
+            "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
+            "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
+        )
+    if reason == "return_baseline_timeout":
+        return (
+            "一个或多个标定电机未在规定时间返回基准命令。",
+            "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "validation_move_timeout":
+        return (
+            "随机复测时电机未在规定时间到达目标命令。",
+            "检查机械手状态和机械卡阻，确认安全后调用resume。",
+        )
+    if reason == "validation_capture_timeout":
+        return (
+            "随机复测位置没有采集到足够的同步有效Tag帧。",
+            "检查当前机位Tag可见性后调用resume。",
+        )
+    if reason == "joint_fit_check_failed":
+        metric_names = {
+            "plane_rms_mm": "平面拟合RMS",
+            "radial_rms_mm": "圆半径拟合RMS",
+            "radius_mm": "拟合半径",
+            "image_radial_rms_px": "二维圆半径拟合RMS",
+            "image_radial_p95_px": "二维圆半径误差P95",
+            "image_radius_px": "二维拟合半径",
+            "arc_deg": "实测圆弧",
+            "monotonic_correction_deg": "最大单调修正",
+            "hysteresis_deg": "最大正反程差",
+            "cycle_travel_range_deg": "三轮行程差",
+            "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
+            "axis_plane_rms_mm": "三维圆轴向RMS",
+            "axis_radial_rms_mm": "三维圆半径RMS",
+            "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
+            "axis_cycle_difference_deg": "三轮转轴方向极差",
+            "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
+            "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
+            "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
+            "state_image_sync_p95_ms": "图像与电机状态同步误差P95",
+            "tag_valid_rate_percent": "所需Tag同时有效率",
+        }
+        metric_units = {
+            "plane_rms_mm": "mm",
+            "radial_rms_mm": "mm",
+            "radius_mm": "mm",
+            "image_radial_rms_px": "px",
+            "image_radial_p95_px": "px",
+            "image_radius_px": "px",
+            "arc_deg": "°",
+            "monotonic_correction_deg": "°",
+            "hysteresis_deg": "°",
+            "cycle_travel_range_deg": "°",
+            "rotation_orthogonal_rms_deg": "°",
+            "axis_plane_rms_mm": "mm",
+            "axis_radial_rms_mm": "mm",
+            "rotation_circle_axis_difference_deg": "°",
+            "axis_cycle_difference_deg": "°",
+            "third_cycle_axis_holdout_deg": "°",
+            "third_cycle_axis_line_rms_mm": "mm",
+            "third_cycle_trajectory_p95_deg": "°",
+            "state_image_sync_p95_ms": "ms",
+            "tag_valid_rate_percent": "%",
+        }
+        details: list[str] = []
+        for failure in active.get("failures", []):
+            joint = JOINT_NAMES_ZH.get(
+                str(failure.get("joint")), str(failure.get("joint"))
+            )
+            metric = str(failure.get("metric", ""))
+            if metric in metric_names:
+                comparison = str(failure.get("comparison", ""))
+                requirement = "不超过" if comparison == "maximum" else "至少"
+                unit = metric_units[metric]
+                detail = (
+                    f"{joint}的{metric_names[metric]}为"
+                    f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
+                    f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
+                )
+                cycle_travel = failure.get("cycle_travel_deg", [])
+                if cycle_travel:
+                    detail += "（三轮=" + "/".join(
+                        f"{float(value):.2f}°" for value in cycle_travel
+                    ) + "）"
+                details.append(detail)
+            else:
+                cycle = failure.get("cycle")
+                cycle_text = "" if cycle is None else f"第{cycle}轮"
+                details.append(
+                    f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
+                    f"{failure.get('reason', '未知原因')}"
+                )
+        detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
+        return (
+            detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
+            "修正Tag位置、遮挡或机械行程后调用"
+            "/g20_calibration/resume；程序只清除当前失败关节的数据"
+            f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
+        )
+    if reason == "zero_model_validation_failed":
+        reason_names = {
+            "zero_offset_reached_configured_bound": "零位解触及安全边界",
+            "zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
+            "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
+            "zero_offset_cycle_difference_too_large": "三轮零位离散过大",
+            "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
+            "zero_axis_cone_mismatch_too_large": (
+                "父子轴夹角与原始URDF不一致，零位旋转无法解释"
+            ),
+            "zero_phase_axis_line_residual_too_large": (
+                "整段SE(3)运动无法稳定确定平行轴线相位"
+            ),
+            "zero_offset_did_not_improve_with_95pct_confidence": (
+                "第三轮留出验证未以95%置信度改善"
+            ),
+        }
+        details: list[str] = []
+        for failure in active.get("failures", []):
+            joint = JOINT_NAMES_ZH.get(
+                str(failure.get("joint")), str(failure.get("joint"))
+            )
+            if failure.get("metric") == "zero_guard":
+                reason_text = reason_names.get(
+                    str(failure.get("reason")), str(failure.get("reason"))
+                )
+                if "actual_deg" in failure and "limit_deg" in failure:
+                    reason_text += (
+                        f"（估计{float(failure['actual_deg']):+.2f}°，"
+                        f"允许±{float(failure['limit_deg']):.2f}°）"
+                    )
+                details.append(f"{joint}：{reason_text}")
+        return (
+            "轨迹采集已完成，但零位/URDF几何验证失败"
+            + ("（" + "；".join(details) + "）" if details else "")
+            + "。程序没有生成正式JSON或修正URDF。",
+            "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
+            "请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
+        )
+    if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
+        return (
+            "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
+            "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
+        )
+    if reason == "call_start":
+        return (
+            "三机位预检已经通过，等待操作员确认开始。",
+            "清空机械手运动范围后调用/g20_calibration/start。",
+        )
+    if reason == "operator_pause":
+        return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
+    if reason == "operator_abort":
+        return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
+    if reason == "collecting_timestamp_synchronised_tag_centres":
+        return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
+    if reason == "capturing_random_validation_pose":
+        return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
+    if reason in {"calibration_passed", "calibration_complete"}:
+        return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
+    if reason == "quality_failed":
+        return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
+    if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
+        return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
+    if state == "RETURN_BASELINE":
+        return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
+    if state == "FITTING":
+        return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
+    return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
+
+
+def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
+    """Render the complete operator status; the JSON topic remains unchanged."""
+    state = str(payload.get("state", ""))
+    active = payload.get("active", {})
+    reason_zh, action_zh = three_camera_reason_zh(
+        state, str(payload.get("reason", "")), active
+    )
+    service_prefix = str(payload.get("service_prefix", "/g20_calibration"))
+    if service_prefix != "/g20_calibration":
+        reason_zh = reason_zh.replace("/g20_calibration", service_prefix)
+        action_zh = action_zh.replace("/g20_calibration", service_prefix)
+    progress = float(payload.get("progress", 0.0))
+    completed = payload.get("completed_sweeps", 0)
+    total = payload.get("total_sweeps", 0)
+    executed = int(payload.get("executed_sweep_directions", completed))
+    scan_progress = float(
+        payload.get(
+            "scan_progress",
+            0.0 if not total else float(completed) / float(total),
+        )
+    )
+    lines = [
+        f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
+        f"原因：{reason_zh}",
+        f"建议：{action_zh}",
+        f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
+        f"扫描进度{scan_progress:.1%}）",
+        f"当前任务：{_task_text(active)}",
+    ]
+    if executed > int(completed):
+        lines.append(
+            f"实际采集：已启动{executed}个方向（含自动重扫）；"
+            f"计划进度只统计{total}个唯一方向，重扫不会重复增加计划进度"
+        )
+    attempt = int(active.get("attempt", active.get("fit_attempt", 1)))
+    if attempt > 1:
+        lines.append(
+            f"整关节重采：当前为第{attempt}/"
+            f"{active.get('fit_attempt_limit', '?')}次采集结果"
+        )
+    if state == "RETURN_BASELINE":
+        baseline_command = payload.get("baseline_command_u8", [])
+        return_command = payload.get("return_command_u8", baseline_command)
+        label = "恢复姿态" if return_command != baseline_command else "基准姿态"
+        lines.append(f"正在确认{label}：{return_command}")
+    if active and active.get("kind") not in {
+        "fit_failure",
+        "zero_model_failure",
+        "motion_stall",
+    }:
+        retry_count = int(active.get("automatic_retry_count", 0))
+        if retry_count:
+            lines.append(
+                "自动重试：当前方向已自动重扫"
+                f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
+                f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
+                f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
+            )
+        sample = active.get("sample", {})
+        motion_progress = active.get("motion_progress")
+        motion_text = (
+            "未知" if motion_progress is None else f"{float(motion_progress):.1%}"
+        )
+        lines.append(
+            "运动采样："
+            f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
+            f"实际{_format_u8(active.get('actual_u8'))}，"
+            f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
+            f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
+            f"{_format_u8(sample.get('maximum_u8'))}"
+        )
+        auxiliary = active.get("auxiliary_motors", [])
+        if auxiliary:
+            lines.append(
+                "避挡姿态："
+                + "，".join(
+                    f"电机{item.get('motor_index')}目标"
+                    f"{item.get('command_u8')}、实际"
+                    f"{_format_u8(item.get('actual_u8'))}"
+                    for item in auxiliary
+                )
+            )
+        speed = active.get("speed", {})
+        if speed:
+            lines.append(
+                "阶段速度：五指目标"
+                f"{speed.get('commanded_finger_speed')}，SDK报告"
+                f"{speed.get('reported_finger_speed')}"
+            )
+        if active.get("sweep_timeout_seconds") is not None:
+            lines.append(
+                "运动保护：扫描超时"
+                f"{float(active['sweep_timeout_seconds']):.1f}s，"
+                "连续"
+                f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
+                "进展不足"
+                f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
+                "则立即暂停"
+            )
+    lines.append("机位：")
+    for name, view in payload.get("views", {}).items():
+        missing = view.get("missing_tag_ids", [])
+        missing_text = "无" if not missing else ",".join(map(str, missing))
+        lines.append(
+            f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
+            f"{'就绪' if view.get('ready') else '等待'}，"
+            f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，"
+            f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
+            f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
+            f"当前缺失Tag={missing_text}"
+        )
+    extrinsics_error = payload.get("camera_extrinsics_error")
+    if extrinsics_error:
+        lines.append(f"外参文件：{extrinsics_error}")
+    lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
+    lines.append(
+        f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
+    )
+    return "\n".join(lines)

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/calibrated_joint_state_bridge.launch.py

@@ -0,0 +1,36 @@
+"""Publish calibrated URDF joint angles from raw u8 commands."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("hand_model", default_value="G20"),
+            DeclareLaunchArgument("hand_type", default_value="right"),
+            DeclareLaunchArgument("calibration_file"),
+            DeclareLaunchArgument("input_topic", default_value=""),
+            DeclareLaunchArgument("output_topic", default_value=""),
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="calibrated_joint_state_bridge",
+                name="calibrated_joint_state_bridge",
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "hand_model": LaunchConfiguration("hand_model"),
+                        "hand_type": LaunchConfiguration("hand_type"),
+                        "calibration_file": LaunchConfiguration(
+                            "calibration_file"
+                        ),
+                        "input_topic": LaunchConfiguration("input_topic"),
+                        "output_topic": LaunchConfiguration("output_topic"),
+                    }
+                ],
+            ),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py

@@ -0,0 +1,272 @@
+"""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,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+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)
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
+    )
+
+    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(
+        [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
+            ),
+            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(
+                "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,276 @@
+"""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,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+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)
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
+    )
+
+    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(
+        [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
+            ),
+            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(
+                "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,391 @@
+"""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,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+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)
+    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")
+
+    try:
+        image_width = int(LaunchConfiguration("image_width").perform(context))
+        image_height = int(LaunchConfiguration("image_height").perform(context))
+    except ValueError as error:
+        raise RuntimeError("image_width and image_height must be integers") from error
+    if image_width <= 0 or image_height <= 0:
+        raise RuntimeError("image_width and image_height must be positive")
+    if use_roi:
+        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 camera image "
+                f"{image_width}x{image_height}"
+            )
+
+    camera = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
+    )
+
+    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,
+            "/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(
+        [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
+            ),
+            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(
+                "tag_config",
+                default_value=str(package_share / "config" / "front_tags.yaml"),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py

@@ -0,0 +1,519 @@
+"""Launch three Hikrobot views and one supported-hand calibration owner."""
+
+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,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+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
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    get_hand_calibration_profile,
+)
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _default_source_urdf(hand_model: str, hand_type: str) -> Path:
+    if hand_model == "O30":
+        relative = (
+            Path("linkerhand-urdf/O30/urdf_0803-right/src")
+            / "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
+        )
+        candidates = (
+            Path.cwd().parent / relative,
+            Path.home() / "projects" / relative,
+        )
+        return next(
+            (candidate for candidate in candidates if candidate.is_file()),
+            Path.home()
+            / "projects"
+            / "linkerhand-urdf/O30/urdf_0803-right/src"
+            / "linkerhand_O30i_right.urdf"
+            / "linkerhand_O30i_right-0803.urdf",
+        )
+    relative = Path(
+        "assets/robots/hands/linker_hand"
+    ) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
+    workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
+    try:
+        installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
+    except Exception:
+        installed = workspace
+    return workspace if workspace.is_file() else installed
+
+
+def _launch_stack(context):
+    hand_model = LaunchConfiguration("hand_model").perform(context).upper()
+    hand_type = LaunchConfiguration("hand_type").perform(context).lower()
+    try:
+        profile = get_hand_calibration_profile(hand_type, hand_model)
+    except ValueError as error:
+        raise RuntimeError(str(error)) from error
+    model_key = hand_model.lower()
+    calibration_namespace = f"/{model_key}_calibration"
+    if hand_model == "O30":
+        command_topic = f"/cb_{hand_type}_hand_control_cmd"
+        state_topic = f"/cb_{hand_type}_hand_state"
+        hand_info_topic = f"/cb_{hand_type}_hand_info"
+        setting_topic = "/cb_hand_setting_cmd"
+        tag_config = LaunchConfiguration("o30_tag_config")
+    else:
+        command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
+        state_topic = f"/g20/cb_{hand_type}_hand_state"
+        hand_info_topic = f"/g20/cb_{hand_type}_hand_info"
+        setting_topic = "/g20/cb_hand_setting_cmd"
+        tag_config = LaunchConfiguration("tag_config")
+    requested_source = LaunchConfiguration("source_urdf_path").perform(context)
+    source_urdf = (
+        Path(requested_source).expanduser().resolve()
+        if requested_source
+        else _default_source_urdf(hand_model, hand_type).resolve()
+    )
+    if not source_urdf.is_file():
+        raise RuntimeError(f"source URDF does not exist: {source_urdf}")
+
+    hand_serial = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not hand_serial
+        or hand_serial == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None
+        or hand_serial in {".", ".."}
+    ):
+        raise RuntimeError("serial_number must be a safe non-empty hand serial")
+
+    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:
+        session_dir = (
+            output_root
+            / hand_serial
+            / datetime.now().strftime("%Y%m%d_%H%M%S")
+        )
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    camera_serials = {
+        view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        for view in VIEWS
+    }
+    if any(not serial for serial in camera_serials.values()):
+        raise RuntimeError("all three camera serial numbers are required")
+    if len(set(camera_serials.values())) != 3:
+        raise RuntimeError("front/side/top camera serial numbers must be unique")
+
+    cameras = []
+    components = []
+    raw_topics = []
+    info_topics = []
+    detection_topics = []
+    for view in VIEWS:
+        namespace = f"{calibration_namespace}/{view}/camera"
+        raw_topic = f"{namespace}/image_raw"
+        camera_info_topic = f"{namespace}/camera_info"
+        rect_topic = f"{namespace}/image_rect"
+        detector_namespace = f"{calibration_namespace}/{view}/apriltag"
+        detection_topic = f"{detector_namespace}/detections"
+        raw_topics.append(raw_topic)
+        info_topics.append(camera_info_topic)
+        detection_topics.append(detection_topic)
+        cameras.append(
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                condition=IfCondition(LaunchConfiguration("start_cameras")),
+                parameters=[
+                    {
+                        "serial_number": LaunchConfiguration(
+                            f"{view}_camera_serial"
+                        ),
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": (
+                            f"{model_key}_calibration_{view}_optical_frame"
+                        ),
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": ParameterValue(
+                            LaunchConfiguration("auto_exposure"), value_type=bool
+                        ),
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        components.extend(
+            [
+                ComposableNode(
+                    package="image_proc",
+                    plugin="image_proc::RectifyNode",
+                    name=f"rectify_{view}",
+                    namespace=namespace,
+                    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=detector_namespace,
+                    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}],
+                ),
+            ]
+        )
+
+    vision = ComposableNodeContainer(
+        name=f"{model_key}_three_camera_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=components,
+        output="screen",
+        emulate_tty=True,
+    )
+    if hand_model == "O30":
+        sdk = Node(
+            package="linker_hand_o30_ros2_sdk",
+            executable="linker_hand_o30_ros2_sdk",
+            name="linker_hand_o30_ros2_sdk",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[
+                {
+                    "hand_type": hand_type,
+                    "hand_joint": "O30",
+                    "is_touch": False,
+                    "canfd_device": ParameterValue(
+                        LaunchConfiguration("canfd_device"), value_type=int
+                    ),
+                    "comm_type": LaunchConfiguration("o30_comm_type"),
+                    "channel": LaunchConfiguration("can_interface"),
+                    "bitrate": ParameterValue(
+                        LaunchConfiguration("o30_bitrate"), value_type=int
+                    ),
+                    "dbitrate": ParameterValue(
+                        LaunchConfiguration("o30_dbitrate"), value_type=int
+                    ),
+                    "auto_setup": ParameterValue(
+                        LaunchConfiguration("o30_auto_setup"), value_type=bool
+                    ),
+                }
+            ],
+        )
+    else:
+        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": hand_type,
+                    "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": 30.0,
+                    "velocity_poll_rate": 1.0,
+                    "defer_state_reads_while_commanding": False,
+                    "repeat_position_commands": False,
+                    "is_touch": False,
+                }
+            ],
+        )
+    calibration = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="three_camera_calibration_node",
+        name=f"{model_key}_calibration",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            LaunchConfiguration("calibration_config"),
+            {
+                "serial_number": hand_serial,
+                "hand_model": hand_model,
+                "hand_type": hand_type,
+                "session_dir": str(session_dir),
+                # The SDK performs roughly 25 synchronous CAN queries whenever
+                # cb_<side>_hand_info has a subscriber. Calibration only used
+                # that topic to display a speed diagnostic, while those reads
+                # created 17-33 command-unit holes in position trajectories.
+                "info_topic": f"{calibration_namespace}/disabled_hand_info",
+                "command_topic": command_topic,
+                "state_topic": state_topic,
+                "setting_topic": setting_topic,
+                "baseline_command_u8": list(profile.baseline_command),
+                "camera_extrinsics_file": LaunchConfiguration(
+                    "camera_extrinsics_file"
+                ),
+                "source_urdf_path": str(source_urdf),
+                "corrected_urdf_output_dir": LaunchConfiguration(
+                    "corrected_urdf_output_dir"
+                ),
+                **{
+                    f"{view}_camera_serial": camera_serials[view]
+                    for view in VIEWS
+                },
+                **{
+                    f"{view}_camera_info_topic": info_topics[index]
+                    for index, view in enumerate(VIEWS)
+                },
+                **{
+                    f"{view}_detections_topic": detection_topics[index]
+                    for index, view in enumerate(VIEWS)
+                },
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"), value_type=bool
+                ),
+                "normal_calibration_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"), value_type=int
+                ),
+                "index_roll_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_roll_calibration_speed"),
+                    value_type=int,
+                ),
+                "index_flex_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_flex_calibration_speed"),
+                    value_type=int,
+                ),
+                "o30_internal_speed_u8": ParameterValue(
+                    LaunchConfiguration("o30_internal_speed_u8"), value_type=int
+                ),
+                "o30_command_full_range_seconds": ParameterValue(
+                    LaunchConfiguration("o30_command_full_range_seconds"),
+                    value_type=float,
+                ),
+                "validation_enabled": ParameterValue(
+                    LaunchConfiguration("validation_enabled"), value_type=bool
+                ),
+            },
+        ],
+    )
+    bag = ExecuteProcess(
+        condition=IfCondition(LaunchConfiguration("record_bag")),
+        cmd=[
+            "ros2",
+            "bag",
+            "record",
+            "--storage",
+            "mcap",
+            "--storage-preset-profile",
+            "zstd_fast",
+            "--max-bag-size",
+            "10737418240",
+            "--output",
+            str(session_dir / "rosbag"),
+            *raw_topics,
+            *info_topics,
+            *detection_topics,
+            command_topic,
+            state_topic,
+            hand_info_topic,
+            f"{calibration_namespace}/status",
+        ],
+        output="screen",
+    )
+    return [
+        LogInfo(
+            msg=(
+                f"{hand_model} {hand_type} three-camera session: {session_dir}; "
+                f"source_urdf={source_urdf}"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Camera mapping: front="
+                f"{camera_serials['front']} side={camera_serials['side']} "
+                f"top={camera_serials['top']}"
+            )
+        ),
+        *cameras,
+        vision,
+        sdk,
+        calibration,
+        bag,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    info_root = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Camera processes publish ~2 MB frames across DDS.  Force the
+            # matching RMW and provide both current and legacy profile names
+            # so the configured 64 MB shared-memory segment is actually used.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            DeclareLaunchArgument("hand_model", default_value="G20"),
+            DeclareLaunchArgument("hand_type", default_value="left"),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "front_camera_serial", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_serial", default_value="DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_serial", default_value="DB2163739"
+            ),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument(
+                "front_camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_name", default_value="hikrobot_side_DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_name", default_value="hikrobot_top_DB2163739"
+            ),
+            DeclareLaunchArgument(
+                "front_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163742.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "side_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163749.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "top_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163739.yaml"),
+            ),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("canfd_device", default_value="0"),
+            DeclareLaunchArgument("o30_comm_type", default_value="socketcan"),
+            DeclareLaunchArgument("o30_bitrate", default_value="1000000"),
+            DeclareLaunchArgument("o30_dbitrate", default_value="5000000"),
+            DeclareLaunchArgument("o30_auto_setup", default_value="true"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument(
+                "index_roll_calibration_speed", default_value="5"
+            ),
+            DeclareLaunchArgument(
+                "index_flex_calibration_speed", default_value="10"
+            ),
+            DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
+            DeclareLaunchArgument(
+                "o30_command_full_range_seconds", default_value="6.0"
+            ),
+            DeclareLaunchArgument("validation_enabled", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_extrinsics_file",
+                default_value=str(
+                    Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "source_urdf_path", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "corrected_urdf_output_dir", default_value=""
+            ),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument("start_cameras", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument("record_bag", default_value="false"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_calibration.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_tags.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "o30_tag_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_tags_o30.yaml"
+                ),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py

@@ -0,0 +1,199 @@
+"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
+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
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _launch(context):
+    cameras = []
+    rectifiers = []
+    serials = {}
+    for view in VIEWS:
+        serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        if not serial:
+            raise RuntimeError(f"{view}_camera_serial is required")
+        serials[view] = serial
+        namespace = f"/g20_extrinsics/{view}/camera"
+        cameras.append(
+            Node(
+                package="g20_thumb_apriltag_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "serial_number": serial,
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": f"g20_extrinsics_{view}_optical_frame",
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": False,
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        rectifiers.append(
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name=f"rectify_{view}",
+                namespace=namespace,
+                remappings=[
+                    ("image", f"{namespace}/image_raw"),
+                    ("camera_info", f"{namespace}/camera_info"),
+                    ("image_rect", f"{namespace}/image_rect"),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            )
+        )
+    container = ComposableNodeContainer(
+        name="g20_extrinsics_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=rectifiers,
+        output="screen",
+    )
+    solver = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="three_camera_extrinsics_node",
+        name="g20_camera_extrinsics",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {
+                "output_file": LaunchConfiguration("output_file"),
+                "checkerboard_columns": ParameterValue(
+                    LaunchConfiguration("checkerboard_columns"), value_type=int
+                ),
+                "checkerboard_rows": ParameterValue(
+                    LaunchConfiguration("checkerboard_rows"), value_type=int
+                ),
+                "square_size_m": ParameterValue(
+                    LaunchConfiguration("square_size_m"), value_type=float
+                ),
+                "enable_gui": ParameterValue(
+                    LaunchConfiguration("enable_gui"), value_type=bool
+                ),
+                "gui_refresh_hz": ParameterValue(
+                    LaunchConfiguration("gui_refresh_hz"), value_type=float
+                ),
+                "maximum_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration("maximum_reprojection_rms_px"),
+                    value_type=float,
+                ),
+                "maximum_single_camera_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration(
+                        "maximum_single_camera_reprojection_rms_px"
+                    ),
+                    value_type=float,
+                ),
+                "auto_capture_default": ParameterValue(
+                    LaunchConfiguration("auto_capture_default"),
+                    value_type=bool,
+                ),
+                "auto_capture_stable_seconds": ParameterValue(
+                    LaunchConfiguration("auto_capture_stable_seconds"),
+                    value_type=float,
+                ),
+                **{
+                    f"{view}_camera_serial": serials[view]
+                    for view in VIEWS
+                },
+            }
+        ],
+    )
+    return [*cameras, container, solver]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    camera_info = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Keep the large-image transport deterministic even when the
+            # calling shell selected another ROS 2 RMW implementation.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("front_camera_serial", default_value="DB2163742"),
+            DeclareLaunchArgument("side_camera_serial", default_value="DB2163749"),
+            DeclareLaunchArgument("top_camera_serial", default_value="DB2163739"),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument("front_camera_name", default_value="hikrobot_front_DB2163742"),
+            DeclareLaunchArgument("side_camera_name", default_value="hikrobot_side_DB2163749"),
+            DeclareLaunchArgument("top_camera_name", default_value="hikrobot_top_DB2163739"),
+            DeclareLaunchArgument("front_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163742.yaml")),
+            DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
+            DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
+            DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("checkerboard_columns", default_value="8"),
+            DeclareLaunchArgument("checkerboard_rows", default_value="5"),
+            DeclareLaunchArgument("square_size_m", default_value="0.027"),
+            DeclareLaunchArgument("enable_gui", default_value="true"),
+            DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
+            DeclareLaunchArgument(
+                "maximum_reprojection_rms_px", default_value="1.2"
+            ),
+            DeclareLaunchArgument(
+                "maximum_single_camera_reprojection_rms_px",
+                default_value="1.5",
+            ),
+            DeclareLaunchArgument("auto_capture_default", default_value="false"),
+            DeclareLaunchArgument(
+                "auto_capture_stable_seconds", default_value="1.0"
+            ),
+            DeclareLaunchArgument(
+                "output_file",
+                default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
+            ),
+            OpaqueFunction(function=_launch),
+        ]
+    )

src/g20_thumb_apriltag_calibration/package.xml

@@ -0,0 +1,36 @@
+<?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>Three-view Hikrobot AprilTag calibration for G20 hands and the O30 right hand.</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>linker_hand_o30_ros2_sdk</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>rclpy</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>
+  <exec_depend>python3-yaml</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,69 @@
+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") + glob("config/*.xml"),
+        ),
+        ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
+    ],
+    install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
+    tests_require=["pytest"],
+    zip_safe=True,
+    maintainer="lxp",
+    maintainer_email="support@linker-robotics.com",
+    description="Three-view AprilTag calibration for G20 and O30 hands",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            (
+                "hikrobot_camera_node = "
+                "g20_thumb_apriltag_calibration.hikrobot_camera:main"
+            ),
+            "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"
+            ),
+            (
+                "three_camera_calibration_node = "
+                "g20_thumb_apriltag_calibration.three_camera_node:main"
+            ),
+            (
+                "three_camera_extrinsics_node = "
+                "g20_thumb_apriltag_calibration.extrinsics_node:main"
+            ),
+            (
+                "offline_replay = "
+                "g20_thumb_apriltag_calibration.offline_replay:main"
+            ),
+            (
+                "camera_alignment_view = "
+                "g20_thumb_apriltag_calibration.alignment_view:main"
+            ),
+            (
+                "calibrated_joint_state_bridge = "
+                "g20_thumb_apriltag_calibration."
+                "calibrated_joint_state_bridge:main"
+            ),
+        ],
+    },
+)

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

@@ -0,0 +1,432 @@
+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,
+    required_resume_views,
+    tag_quality_is_valid,
+    update_pnp_reset_watchdog,
+)
+from g20_thumb_apriltag_calibration.core import PAIR_NAMES
+
+
+def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
+    since, reset = update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=False,
+        now=10.0,
+        invalid_since=None,
+        reset_after_seconds=1.0,
+    )
+    assert since == 10.0
+    assert reset is False
+
+    since, reset = update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=False,
+        now=11.01,
+        invalid_since=since,
+        reset_after_seconds=1.0,
+    )
+    assert since == 11.01
+    assert reset is True
+
+
+def test_pnp_watchdog_clears_on_valid_pose_or_bad_detection() -> None:
+    assert update_pnp_reset_watchdog(
+        detection_good=True,
+        pnp_valid=True,
+        now=11.0,
+        invalid_since=10.0,
+        reset_after_seconds=1.0,
+    ) == (None, False)
+    assert update_pnp_reset_watchdog(
+        detection_good=False,
+        pnp_valid=False,
+        now=11.0,
+        invalid_since=10.0,
+        reset_after_seconds=1.0,
+    ) == (None, False)
+
+
+def test_resume_requires_only_the_active_view() -> None:
+    assert required_resume_views("top") == ("top",)
+    assert required_resume_views("front") == ("front",)
+    assert required_resume_views(None) == ("front", "side", "top")
+    with pytest.raises(ValueError, match="unknown"):
+        required_resume_views("rear")
+
+
+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_alignment_view.py

@@ -0,0 +1,39 @@
+"""Tests for the independent physical-line alignment overlay."""
+
+import pytest
+
+from g20_thumb_apriltag_calibration.alignment_view import (
+    summarize_alignment_measurements,
+)
+
+
+def _measurement(angle: float, offset: float, y: float) -> dict:
+    return {
+        "line_xyxy_px": [0.0, y, 100.0, y - angle * 100.0],
+        "angle_rad": angle,
+        "vertical_offset_px": offset,
+    }
+
+
+def test_line_summary_smooths_only_physical_line_measurements() -> None:
+    """The overlay smooths scene lines without any Tag orientation input."""
+    result = summarize_alignment_measurements(
+        [
+            _measurement(-0.02, -4.0, 80.0),
+            None,
+            _measurement(0.00, 0.0, 82.0),
+            _measurement(0.02, 4.0, 84.0),
+        ]
+    )
+
+    assert result is not None
+    assert result["angle_rad"] == pytest.approx(0.0)
+    assert result["vertical_offset_px"] == pytest.approx(0.0)
+    assert result["line_xyxy_px"] == pytest.approx([0.0, 82.0, 100.0, 82.0])
+    assert result["detected_frames"] == 3
+    assert result["window_frames"] == 4
+
+
+def test_line_summary_returns_none_without_scene_line() -> None:
+    """No blue line is fabricated when the scene has no valid long edge."""
+    assert summarize_alignment_measurements([None, None]) is None

src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py

@@ -0,0 +1,131 @@
+import copy
+
+import pytest
+
+from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
+    G20_COMMAND_NAMES,
+    G20_URDF_JOINT_NAMES,
+    O30_COMMAND_NAMES,
+    O30_URDF_JOINT_NAMES,
+    CalibratedCommandMapper,
+)
+from g20_thumb_apriltag_calibration.full_hand import (
+    JointCurveFit,
+    build_compact_payload,
+    get_hand_calibration_profile,
+)
+
+
+def _payload(side: str = "right") -> dict:
+    profile = get_hand_calibration_profile(side)
+    curve = [round((255 - command) * 0.001, 8) for command in range(256)]
+    joints = {}
+    for name, spec in profile.joint_specs.items():
+        joint = {
+            "motor_index": spec.motor_index,
+            "angle_rad": list(curve),
+        }
+        if spec.active:
+            joint["zero_command_u8"] = 255
+            joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
+        else:
+            joint["passive"] = True
+        if spec.source_joint is not None:
+            joint["source_joint"] = spec.source_joint
+        joints[name] = joint
+    return {
+        "schema_version": 4,
+        "model": "G20",
+        "side": side,
+        "serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
+        "angle_unit": "rad",
+        "command_range": [0, 255],
+        "baseline_command_u8": [255] * 20,
+        "joints": joints,
+        "quality": {
+            "passed": True,
+            "validation_mae_rad": 0.01,
+            "validation_p95_rad": 0.02,
+        },
+    }
+
+
+def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    command = list(range(20))
+    result = dict(
+        zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
+    )
+
+    assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
+    assert result["thumb_cmc_roll"] == pytest.approx(0.250)
+    assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
+    assert result["thumb_mcp"] == pytest.approx(0.240)
+    assert result["thumb_ip"] == pytest.approx(0.240)
+    assert result["pinky_pip"] == pytest.approx(0.236)
+    assert result["pinky_dip"] == pytest.approx(0.236)
+
+
+def test_mapper_uses_names_instead_of_message_order() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    command = list(range(20))
+    names = list(reversed(G20_COMMAND_NAMES))
+    positions = list(reversed(command))
+
+    assert mapper.map_positions(positions, names) == mapper.map_positions(command)
+
+
+def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
+    with pytest.raises(ValueError, match="does not match"):
+        CalibratedCommandMapper(_payload("left"), expected_side="right")
+
+    payload = copy.deepcopy(_payload())
+    payload["quality"]["passed"] = False
+    with pytest.raises(ValueError, match="quality.passed"):
+        CalibratedCommandMapper(payload, expected_side="right")
+
+
+def test_mapper_rejects_incomplete_named_command() -> None:
+    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
+    with pytest.raises(ValueError, match="missing named channels"):
+        mapper.map_positions([255.0], ["thumb_cmc_pitch"])
+
+
+def test_o30_mapper_uses_sdk_names_and_twenty_active_urdf_joints() -> None:
+    profile = get_hand_calibration_profile("right", "O30")
+
+    def fit_for(name: str) -> JointCurveFit:
+        zero = profile.baseline_command[profile.joint_specs[name].motor_index]
+        curve = tuple((command - zero) * 0.001 for command in range(256))
+        return JointCurveFit(
+            angle_rad=curve,
+            decreasing_rad=curve,
+            increasing_rad=curve,
+            circle={},
+            maximum_monotonic_correction_rad=0.0,
+            maximum_hysteresis_rad=0.0,
+            quality={},
+        )
+
+    payload = build_compact_payload(
+        serial_number="O30_RIGHT_TEST",
+        measured_fits={name: fit_for(name) for name in profile.measured_joints},
+        urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
+        validation_errors_rad=[0.0],
+        passed=True,
+        side="right",
+        model="O30",
+    )
+    mapper = CalibratedCommandMapper(payload, expected_side="right")
+
+    assert mapper.model == "O30"
+    assert mapper.command_names == O30_COMMAND_NAMES
+    assert mapper.urdf_joint_names == O30_URDF_JOINT_NAMES
+    baseline = profile.baseline_command
+    assert mapper.map_positions(baseline, O30_COMMAND_NAMES) == pytest.approx(
+        [0.0] * 20
+    )
+    moved = list(baseline)
+    moved[19] = 100
+    result = dict(zip(O30_URDF_JOINT_NAMES, mapper.map_positions(moved)))
+    assert result["pinky_dip"] == pytest.approx(0.1)

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -0,0 +1,341 @@
+from pathlib import Path
+from xml.etree import ElementTree
+
+import yaml
+
+
+PACKAGE_ROOT = Path(__file__).resolve().parents[1]
+
+
+def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
+    root = ElementTree.parse(
+        PACKAGE_ROOT / "config" / "fastdds_large_images.xml"
+    ).getroot()
+    namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"}
+    profiles = root.find("dds:profiles", namespace)
+    assert profiles is not None
+    segment = profiles.find(
+        ".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size",
+        namespace,
+    )
+    assert segment is not None
+    assert int(segment.text) >= 64 * 1024 * 1024
+    participant = profiles.find("dds:participant", namespace)
+    assert participant is not None
+    assert participant.attrib["is_default_profile"] == "true"
+
+
+def test_three_camera_launches_force_fastdds_large_image_transport() -> None:
+    for launch_name in (
+        "three_camera_calibration.launch.py",
+        "three_camera_extrinsics.launch.py",
+    ):
+        launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text()
+        assert 'name="RMW_IMPLEMENTATION"' in launch_text
+        assert 'value="rmw_fastrtps_cpp"' in launch_text
+        assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text
+        assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text
+
+
+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_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
+    expected = {
+        "front": [0, 1, 2, 3, 10],
+        "side": [4, 5, 6, 7],
+        "top": [8, 9],
+    }
+    for model in ("g20", "o30"):
+        suffix = "" if model == "g20" else "_o30"
+        tags = yaml.safe_load(
+            (
+                PACKAGE_ROOT
+                / "config"
+                / f"three_camera_tags{suffix}.yaml"
+            ).read_text()
+        )
+        all_ids = set()
+        for view, ids in expected.items():
+            key = f"/{model}_calibration/{view}/apriltag/apriltag"
+            parameters = tags[key]["ros__parameters"]
+            assert parameters["tag"]["ids"] == ids
+            assert parameters["size"] == 0.016
+            assert parameters["tag"]["sizes"] == [0.016] * len(ids)
+            assert parameters["qos_profile"] == "sensor_data"
+            assert parameters["detector"]["decimate"] == 1.5
+            all_ids.update(ids)
+        assert all_ids == set(range(11))
+        assert tags[
+            f"/{model}_calibration/side/apriltag/apriltag"
+        ]["ros__parameters"]["tag"]["frames"][0] == "side_base"
+
+
+def test_three_camera_launch_selects_o30_tag_parameters() -> None:
+    launch_text = (
+        PACKAGE_ROOT / "launch" / "three_camera_calibration.launch.py"
+    ).read_text()
+
+    assert 'tag_config = LaunchConfiguration("o30_tag_config")' in launch_text
+    assert '"three_camera_tags_o30.yaml"' in launch_text
+
+
+def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
+    )
+    parameters = config["/**"]["ros__parameters"]
+
+    assert parameters["tag_size_m"] == 0.016
+
+    assert parameters["baseline_command_u8"] == [
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        127,
+        127,
+        127,
+        127,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+        255,
+    ]
+    assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd"
+    assert parameters["normal_calibration_speed"] == 15
+    assert parameters["index_roll_calibration_speed"] == 5
+    assert parameters["index_flex_calibration_speed"] == 10
+    assert parameters["o30_internal_speed_u8"] == 0
+    assert parameters["o30_command_full_range_seconds"] == 6.0
+    assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
+    assert parameters["speed_setting_settle_seconds"] >= 0.2
+    assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
+    assert parameters["pnp_group_initialization_frames"] == 8
+    assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
+    assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
+    assert parameters["repetitions"] == 3
+    assert parameters["validation_enabled"] is False
+    assert parameters["minimum_detection_rate"] == 0.95
+    assert parameters["minimum_detection_hz"] == 15.0
+    assert parameters["minimum_state_span_u8"] >= 240.0
+    assert parameters["endpoint_tolerance_u8"] == 2.0
+    assert parameters["o30_endpoint_tolerance_u8"] == 4.0
+    assert (
+        parameters["o30_thumb_cmc_roll_255_endpoint_tolerance_u8"] == 9.0
+    )
+    assert (
+        parameters["o30_index_mcp_roll_255_endpoint_tolerance_u8"] == 8.0
+    )
+    assert parameters["o30_thumb_mcp_zero_endpoint_tolerance_u8"] == 8.0
+    assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
+    assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
+    assert parameters["minimum_sweep_bins"] >= 32
+    assert parameters["maximum_bin_gap"] <= 16
+    assert parameters["automatic_sweep_retry_limit"] == 3
+    assert parameters["automatic_fit_retry_limit"] == 2
+    assert parameters["motor_stall_timeout_seconds"] >= 5.0
+    assert parameters["motor_stall_minimum_progress_u8"] == 1.0
+    assert parameters["automatic_motion_retry_limit"] == 2
+    assert parameters["provisional_warning_ratio"] == 1.25
+    assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
+    assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
+    assert parameters["position_timeout_seconds"] >= 20.0
+    assert parameters["maximum_state_image_skew_ms"] <= 50.0
+    assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
+    assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001
+    assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004
+    assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5
+    assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
+    assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
+    assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
+    assert parameters["zero_maximum_offset_deg"] <= 20.0
+    assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
+    assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
+    assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
+    assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
+    assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
+    assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
+    assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
+    assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
+    for view in ("front", "side", "top"):
+        assert parameters[f"{view}_camera_info_topic"].startswith(
+            f"/g20_calibration/{view}/"
+        )
+        assert parameters[f"{view}_detections_topic"] == (
+            f"/g20_calibration/{view}/apriltag/detections"
+        )
+
+
+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_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_extrinsics_node.py

@@ -0,0 +1,326 @@
+"""Focused tests for checkerboard frame pairing."""
+
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node
+from g20_thumb_apriltag_calibration.extrinsics_node import (
+    BoardPose,
+    StereoCapture,
+    _fit_stereo_robust,
+    _individual_reprojection_passes,
+    _minimum_history_skew_ns,
+    _pose_is_novel,
+    _select_latest_synchronised_pair,
+    _summarize_transform_repeatability,
+)
+
+
+def _pose(stamp_ns: int) -> BoardPose:
+    return BoardPose(
+        stamp_ns=stamp_ns,
+        camera_from_board_candidates=(),
+        reprojection_rms_px=0.1,
+    )
+
+
+def test_pairing_uses_recent_history_instead_of_mismatched_latest_frames():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_030_000_000), _pose(1_370_000_000)]
+
+    pair = _select_latest_synchronised_pair(front, side, 100_000_000)
+
+    assert pair is not None
+    selected_front, selected_side, skew = pair
+    assert selected_front.stamp_ns == 1_000_000_000
+    assert selected_side.stamp_ns == 1_030_000_000
+    assert skew == 30_000_000
+
+
+def test_pairing_prefers_newest_valid_common_pair():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_010_000_000), _pose(1_240_000_000)]
+
+    pair = _select_latest_synchronised_pair(front, side, 100_000_000)
+
+    assert pair is not None
+    selected_front, selected_side, skew = pair
+    assert selected_front.stamp_ns == 1_200_000_000
+    assert selected_side.stamp_ns == 1_240_000_000
+    assert skew == 40_000_000
+
+
+def test_pairing_reports_minimum_skew_when_no_pair_passes():
+    front = [_pose(1_000_000_000), _pose(1_200_000_000)]
+    side = [_pose(1_370_000_000)]
+
+    assert _select_latest_synchronised_pair(
+        front, side, 100_000_000
+    ) is None
+    assert _minimum_history_skew_ns(front, side) == 170_000_000
+
+
+def _transform(rotation_deg: float, translation_m: float) -> np.ndarray:
+    value = np.eye(4)
+    value[:3, :3] = Rotation.from_euler(
+        "z", rotation_deg, degrees=True
+    ).as_matrix()
+    value[0, 3] = translation_m
+    return value
+
+
+def test_repeatability_summary_accepts_consistent_capture_set():
+    captures = [
+        [_transform(-0.1, -0.0005)],
+        [_transform(0.0, 0.0)],
+        [_transform(0.1, 0.0005)],
+    ]
+
+    _, selected, rotation_error, translation_error = (
+        _summarize_transform_repeatability(captures)
+    )
+
+    assert len(selected) == 3
+    np.testing.assert_allclose(
+        np.degrees(rotation_error), 0.1, atol=1.0e-6
+    )
+    np.testing.assert_allclose(translation_error, 0.0005, atol=1.0e-9)
+
+
+def test_repeatability_summary_exposes_current_capture_outlier():
+    captures = [
+        [_transform(0.0, 0.0)],
+        [_transform(0.1, 0.0005)],
+        [_transform(1.0, 0.008)],
+    ]
+
+    _, _, rotation_error, translation_error = (
+        _summarize_transform_repeatability(captures)
+    )
+
+    assert np.degrees(rotation_error) > 0.3
+    assert translation_error > 0.0015
+
+
+def test_each_camera_must_pass_reprojection_gate_independently():
+    assert _individual_reprojection_passes(0.8, 1.1, 1.2)
+    assert not _individual_reprojection_passes(0.5, 1.4, 1.2)
+    assert _individual_reprojection_passes(0.5, 1.4, 1.5)
+    assert not _individual_reprojection_passes(0.5, 1.6, 1.5)
+    assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2)
+
+
+def test_pose_novelty_is_checked_against_all_previous_captures():
+    previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)]
+
+    assert not _pose_is_novel(
+        _transform(10.5, 0.105),
+        previous,
+        minimum_rotation_rad=np.deg2rad(2.0),
+        minimum_translation_m=0.015,
+    )
+    assert _pose_is_novel(
+        _transform(14.0, 0.13),
+        previous,
+        minimum_rotation_rad=np.deg2rad(2.0),
+        minimum_translation_m=0.015,
+    )
+
+
+def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate(
+    monkeypatch,
+):
+    captures = [
+        StereoCapture(
+            front_points_px=np.zeros((40, 2), dtype=np.float32),
+            other_points_px=np.zeros((40, 2), dtype=np.float32),
+            transform_candidates=(np.eye(4),),
+            pair_reprojection_rms_px=1.0,
+            skew_ns=1,
+        )
+        for _ in range(20)
+    ]
+
+    def fake_fit_once(
+        captures_arg,
+        indices,
+        object_points,
+        front_matrix,
+        other_matrix,
+        image_size,
+    ):
+        del (
+            captures_arg,
+            object_points,
+            front_matrix,
+            other_matrix,
+            image_size,
+        )
+        errors = np.asarray(
+            [1.1 if index < 15 else 1.5 for index in indices],
+            dtype=float,
+        )
+        return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors
+
+    monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
+    result = _fit_stereo_robust(
+        captures,
+        np.zeros((40, 3), dtype=np.float32),
+        np.eye(3),
+        np.eye(3),
+        (640, 480),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert result.passed
+    assert result.stereo_rms_px <= 1.2
+    assert len(result.inlier_indices) >= 15
+    assert result.rejected_indices
+    assert set(result.rejected_indices) <= set(range(15, 20))
+
+
+def test_joint_fit_reports_finite_provisional_metrics_before_minimum(
+    monkeypatch,
+):
+    captures = [
+        StereoCapture(
+            front_points_px=np.zeros((40, 2), dtype=np.float32),
+            other_points_px=np.zeros((40, 2), dtype=np.float32),
+            transform_candidates=(np.eye(4),),
+            pair_reprojection_rms_px=0.8,
+            skew_ns=1,
+        )
+        for _ in range(10)
+    ]
+
+    def fake_fit_once(
+        captures_arg,
+        indices,
+        object_points,
+        front_matrix,
+        other_matrix,
+        image_size,
+    ):
+        del (
+            captures_arg,
+            object_points,
+            front_matrix,
+            other_matrix,
+            image_size,
+        )
+        return 0.8, np.eye(4), np.full(len(indices), 0.8)
+
+    monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
+    result = _fit_stereo_robust(
+        captures,
+        np.zeros((40, 3), dtype=np.float32),
+        np.eye(3),
+        np.eye(3),
+        (640, 480),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert not result.passed
+    assert result.stereo_rms_px == 0.8
+    assert len(result.inlier_indices) == 10
+
+
+def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view():
+    random = np.random.default_rng(7)
+    object_points = np.zeros((40, 3), dtype=np.float32)
+    object_points[:, :2] = (
+        np.mgrid[0:8, 0:5].T.reshape(-1, 2) * 0.027
+    )
+    matrix = np.asarray(
+        [[1800.0, 0.0, 812.0], [0.0, 1795.0, 620.0], [0.0, 0.0, 1.0]]
+    )
+    other_from_front = np.eye(4)
+    other_from_front[:3, :3] = Rotation.from_euler(
+        "xyz", [2.0, 18.0, -1.0], degrees=True
+    ).as_matrix()
+    other_from_front[:3, 3] = [0.20, -0.01, 0.04]
+    expected_front_from_other = np.linalg.inv(other_from_front)
+    captures = []
+    for index in range(21):
+        front_from_board = np.eye(4)
+        front_from_board[:3, :3] = Rotation.from_euler(
+            "xyz",
+            [
+                -8.0 + index * 0.7,
+                5.0 + (index % 5) * 2.0,
+                -5.0 + (index % 4) * 3.0,
+            ],
+            degrees=True,
+        ).as_matrix()
+        front_from_board[:3, 3] = [
+            -0.08 + (index % 5) * 0.035,
+            -0.04 + (index % 4) * 0.025,
+            0.75 + (index % 3) * 0.08,
+        ]
+        other_from_board = other_from_front @ front_from_board
+        front_rvec = Rotation.from_matrix(
+            front_from_board[:3, :3]
+        ).as_rotvec()
+        other_rvec = Rotation.from_matrix(
+            other_from_board[:3, :3]
+        ).as_rotvec()
+        front_points, _ = cv2.projectPoints(
+            object_points,
+            front_rvec,
+            front_from_board[:3, 3],
+            matrix,
+            np.zeros(5),
+        )
+        other_points, _ = cv2.projectPoints(
+            object_points,
+            other_rvec,
+            other_from_board[:3, 3],
+            matrix,
+            np.zeros(5),
+        )
+        front_points = front_points.reshape(-1, 2)
+        other_points = other_points.reshape(-1, 2)
+        front_points += random.normal(0.0, 0.12, front_points.shape)
+        other_points += random.normal(0.0, 0.12, other_points.shape)
+        if index == 20:
+            other_points += random.normal(0.0, 4.0, other_points.shape)
+        captures.append(
+            StereoCapture(
+                front_points_px=front_points.astype(np.float32),
+                other_points_px=other_points.astype(np.float32),
+                transform_candidates=(expected_front_from_other.copy(),),
+                pair_reprojection_rms_px=0.2,
+                skew_ns=10_000_000,
+            )
+        )
+
+    result = _fit_stereo_robust(
+        captures,
+        object_points,
+        matrix,
+        matrix,
+        (1624, 1240),
+        minimum_inliers=15,
+        maximum_rms_px=1.2,
+        maximum_rotation_stability_rad=np.deg2rad(0.3),
+        maximum_translation_stability_m=0.0015,
+    )
+
+    assert result.passed
+    assert 20 in result.rejected_indices
+    assert len(result.inlier_indices) >= 15
+    rotation_error = Rotation.from_matrix(
+        result.front_from_other[:3, :3]
+    ).inv() * Rotation.from_matrix(expected_front_from_other[:3, :3])
+    assert np.degrees(rotation_error.magnitude()) < 0.05
+    assert np.linalg.norm(
+        result.front_from_other[:3, 3]
+        - expected_front_from_other[:3, 3]
+    ) < 0.001

src/g20_thumb_apriltag_calibration/test/test_full_hand.py

@@ -0,0 +1,500 @@
+import math
+from dataclasses import replace
+
+import numpy as np
+import pytest
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    ACTIVE_JOINTS,
+    IMAGE_TRAJECTORY_JOINTS,
+    JOINT_SPECS,
+    LEFT_HAND_PROFILE,
+    MEASURED_JOINTS,
+    PASSIVE_JOINTS,
+    RIGHT_HAND_PROFILE,
+    SPLAY_JOINTS,
+    SWEEP_SPECS,
+    VIEW_TAGS,
+    build_calibration_motion_command,
+    build_calibration_speed_profile,
+    build_compact_payload,
+    build_full_hand_command,
+    center_splay_curve,
+    fit_joint_center_curve,
+    fit_joint_image_curve,
+    fit_measured_joint_curve,
+    fit_projected_zero,
+    get_hand_calibration_profile,
+    measure_joint_observation,
+    validate_compact_payload,
+)
+
+
+def _records() -> list[dict[str, object]]:
+    commands = list(range(0, 256, 16))
+    if commands[-1] != 255:
+        commands.append(255)
+    records: list[dict[str, object]] = []
+    centre = np.asarray([0.006, -0.004, 0.012])
+    radius = 0.025
+    image_centre = np.asarray([30.0, -12.0])
+    image_radius = 100.0
+    for cycle in range(3):
+        for direction, sequence in (
+            ("decreasing", reversed(commands)),
+            ("increasing", commands),
+        ):
+            for command in sequence:
+                angle = 0.70 * (255.0 - command) / 255.0
+                point = centre + np.asarray(
+                    [radius * math.cos(angle), radius * math.sin(angle), 0.0]
+                )
+                # At command 255 the inward vector points along image +x, so
+                # table_projected_zero_rad is exactly zero.
+                image_point = image_centre + np.asarray(
+                    [
+                        -image_radius * math.cos(angle),
+                        image_radius * math.sin(angle),
+                    ]
+                )
+                records.append(
+                    {
+                        "cycle": cycle,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_translation_xyz_m": point.tolist(),
+                        "image_relative_xy_px": image_point.tolist(),
+                    }
+                )
+    return records
+
+
+def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
+    assert len(JOINT_SPECS) == 21
+    assert len(ACTIVE_JOINTS) == 16
+    assert len(PASSIVE_JOINTS) == 5
+
+
+def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
+    profile = get_hand_calibration_profile("right")
+    assert profile is RIGHT_HAND_PROFILE
+    assert profile.reference_finger == "pinky"
+    assert [spec.motor_index for spec in profile.sweep_specs] == [
+        0, 5, 15, 9, 4, 19, 10
+    ]
+    assert profile.view_tags["front"]["pinky_roll"] == 10
+    assert profile.view_tags["side"] == {
+        "side_base": 4,
+        "pinky_mcp": 5,
+        "pinky_pip": 6,
+        "pinky_dip": 7,
+    }
+    assert profile.preflight_view_roles["side"] == (
+        "side_base",
+        "pinky_mcp",
+        "pinky_pip",
+        "pinky_dip",
+    )
+    thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
+    assert thumb_pitch.view == "front"
+    assert thumb_pitch.parent_role == "front_base"
+    assert thumb_pitch.child_role == "thumb_cmc"
+    assert profile.joint_specs["index_mcp_roll"].source_joint == (
+        "pinky_mcp_roll"
+    )
+    assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
+        "pinky_mcp_pitch"
+    )
+    assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
+    assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
+
+    roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
+    command = build_calibration_motion_command(
+        roll, 127, profile=profile
+    )
+    assert command[9] == 127
+    assert command[6:9] == [255, 255, 255]
+    speeds = build_calibration_speed_profile(
+        roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    )
+    assert speeds == [15, 15, 15, 15, 5]
+    assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
+        range(11)
+    )
+    assert VIEW_TAGS["front"]["index_roll"] == 10
+    assert VIEW_TAGS["side"] == {
+        "side_base": 4,
+        "index_mcp": 5,
+        "index_pip": 6,
+        "index_dip": 7,
+    }
+    assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
+    assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
+
+
+def test_joint_trajectory_spaces_match_observation_geometry() -> None:
+    assert IMAGE_TRAJECTORY_JOINTS == {
+        "thumb_cmc_pitch",
+        "thumb_cmc_roll",
+        "thumb_mcp",
+        "thumb_ip",
+        "index_mcp_roll",
+        "index_mcp_pitch",
+        "index_pip",
+    }
+    records = _records()
+    for name in IMAGE_TRAJECTORY_JOINTS:
+        fit = fit_measured_joint_curve(name, records)
+        assert fit.circle["space"] == "image_2d"
+    for name in ("index_dip", "thumb_cmc_yaw"):
+        assert "space" not in fit_measured_joint_curve(name, records).circle
+
+
+def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
+    result = build_full_hand_command(6, 27)
+    assert result[6] == 27
+    assert result[:6] == [255] * 6
+    with pytest.raises(ValueError, match="controlled"):
+        build_full_hand_command(11, 27)
+
+
+def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+
+    result = build_calibration_motion_command(index_roll, 27, baseline)
+
+    assert result[6:10] == [27, 0, 0, 0]
+    assert result[:6] == baseline[:6]
+    assert result[10:] == baseline[10:]
+
+
+def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
+    profile = RIGHT_HAND_PROFILE
+    pinky_roll = next(
+        spec for spec in profile.sweep_specs if spec.motor_index == 9
+    )
+
+    result = build_calibration_motion_command(
+        pinky_roll, 27, profile=profile
+    )
+
+    assert result[6:10] == [255, 255, 255, 27]
+    assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
+
+
+def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+
+    result = build_calibration_motion_command(thumb_pitch, 17, baseline)
+
+    assert result[0] == 17
+    assert result[6:10] == [127, 127, 127, 127]
+
+
+def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
+    profile = RIGHT_HAND_PROFILE
+    thumb_pitch = next(
+        spec for spec in profile.sweep_specs if spec.motor_index == 0
+    )
+
+    result = build_calibration_motion_command(
+        thumb_pitch, 17, profile=profile
+    )
+
+    assert result[0] == 17
+    assert result[10] == 255
+    assert result[5] == 255
+    assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
+
+
+def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+
+    result = build_calibration_motion_command(thumb_pitch, 17)
+
+    assert result[0] == 17
+    assert result[5] == 255
+    assert result[10] == 255
+    assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
+
+
+def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
+
+    result = build_calibration_motion_command(thumb_yaw, 27, baseline)
+
+    assert result[5] == 145
+    assert result[10] == 27
+    assert result[:5] == baseline[:5]
+    assert result[6:10] == baseline[6:10]
+    assert result[11:] == baseline[11:]
+
+
+def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+    index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
+
+    assert build_calibration_speed_profile(
+        index_roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 5, 15, 15, 15]
+    assert build_calibration_speed_profile(
+        index_pitch,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 10, 15, 15, 15]
+
+    index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
+    assert build_calibration_speed_profile(
+        index_pip,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 10, 15, 15, 15]
+
+    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
+    assert build_calibration_speed_profile(
+        thumb_pitch,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+    ) == [15, 15, 15, 15, 15]
+
+
+def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
+    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
+    with pytest.raises(ValueError, match="speeds"):
+        build_calibration_speed_profile(
+            index_roll,
+            normal_speed=15,
+            index_roll_speed=256,
+            index_flex_speed=10,
+        )
+
+
+def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
+    records = _records()
+    fit = fit_joint_center_curve(records)
+    assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
+    assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
+    assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
+    assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
+
+
+def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
+    records = _records()
+    for record in records:
+        command = int(record["command_u8"])
+        record["relative_translation_xyz_m"] = [
+            0.001 * command,
+            0.0,
+            0.0,
+        ]
+
+    fit = fit_measured_joint_curve("thumb_mcp", records)
+
+    assert fit.circle["space"] == "image_2d"
+    assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
+    assert fit.angle_rad[255] == pytest.approx(0.0)
+    assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
+    assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
+
+    command_zero = next(
+        record for record in records if int(record["command_u8"]) == 0
+    )
+    observed = measure_joint_observation(
+        fit,
+        vector_xyz_m=command_zero["relative_translation_xyz_m"],
+        image_vector_xy_px=command_zero["image_relative_xy_px"],
+    )
+    assert observed == pytest.approx(0.70, abs=0.02)
+
+
+def test_image_curve_rejects_insufficient_projected_arc() -> None:
+    records = _records()
+    for record in records:
+        command = float(record["command_u8"])
+        angle = math.radians(2.0) * (255.0 - command) / 255.0
+        record["image_relative_xy_px"] = [
+            100.0 * math.cos(angle),
+            100.0 * math.sin(angle),
+        ]
+
+    with pytest.raises(
+        ValueError, match="joint_image_trajectory_quality_failed:arc"
+    ):
+        fit_joint_image_curve(records)
+
+
+def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
+    fit = fit_joint_center_curve(_records())
+    centred, zero_command, midpoint = center_splay_curve(fit)
+    assert midpoint == pytest.approx(0.35, abs=1.0e-4)
+    assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
+    assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
+    assert zero_command in {127, 128}
+    assert abs(centred.angle_rad[zero_command]) <= 0.002
+
+
+def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "index_mcp_roll" else base
+        for name in MEASURED_JOINTS
+    }
+    offsets = {name: 0.01 for name in ACTIVE_JOINTS}
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+    payload = build_compact_payload(
+        serial_number="G20_LEFT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.01, -0.02],
+        passed=True,
+        baseline=baseline,
+    )
+    validate_compact_payload(payload)
+    assert set(payload) == {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "angle_unit",
+        "command_range",
+        "baseline_command_u8",
+        "joints",
+        "quality",
+    }
+    assert len(payload["joints"]) == 21
+    assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
+    assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
+    assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
+    assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
+    assert (
+        payload["joints"]["middle_mcp_roll"]["angle_rad"]
+        == payload["joints"]["index_mcp_roll"]["angle_rad"]
+    )
+    assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
+        "urdf_zero_offset_rad": pytest.approx(0.01)
+    }
+    for name in SPLAY_JOINTS:
+        assert payload["joints"][name]["zero_command_u8"] == zero_command
+
+
+def test_compact_payload_allows_skipped_random_validation() -> None:
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "index_mcp_roll" else base
+        for name in MEASURED_JOINTS
+    }
+    offsets = {name: 0.0 for name in ACTIVE_JOINTS}
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+
+    payload = build_compact_payload(
+        serial_number="G20_LEFT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[],
+        passed=True,
+        baseline=baseline,
+    )
+
+    validate_compact_payload(payload)
+    assert payload["quality"] == {
+        "passed": True,
+        "validation_mae_rad": None,
+        "validation_p95_rad": None,
+    }
+
+
+def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
+    profile = RIGHT_HAND_PROFILE
+    base = fit_joint_center_curve(_records())
+    splay, zero_command, _ = center_splay_curve(base)
+    splay = replace(
+        splay,
+        angle_rad=tuple(
+            value - splay.angle_rad[zero_command]
+            for value in splay.angle_rad
+        ),
+    )
+    measured = {
+        name: splay if name == "pinky_mcp_roll" else base
+        for name in profile.measured_joints
+    }
+    offsets = {
+        name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
+        for name in profile.active_joints
+    }
+    baseline = [255] * 20
+    baseline[6:10] = [zero_command] * 4
+    payload = build_compact_payload(
+        serial_number="G20_RIGHT_001",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.01],
+        passed=True,
+        baseline=baseline,
+        side="right",
+    )
+
+    validate_compact_payload(payload)
+    assert payload["schema_version"] == 4
+    assert payload["side"] == "right"
+    assert set(payload) == {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "angle_unit",
+        "command_range",
+        "baseline_command_u8",
+        "joints",
+        "quality",
+    }
+    for finger in ("index", "middle", "ring"):
+        assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
+            "pinky_mcp_roll"
+        )
+        assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
+            "pinky_mcp_pitch"
+        )
+        assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
+            "pinky_pip"
+        )
+        for suffix in ("mcp_roll", "mcp_pitch", "pip"):
+            joint = payload["joints"][f"{finger}_{suffix}"]
+            source = payload["joints"][joint["source_joint"]]
+            assert joint["angle_rad"] == source["angle_rad"]
+            assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
+            assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}

src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py

@@ -0,0 +1,104 @@
+from pathlib import Path
+
+import pytest
+import yaml
+
+from g20_thumb_apriltag_calibration.hikrobot_camera import (
+    DeviceDescriptor,
+    decode_c_string,
+    load_camera_calibration,
+    resolve_camera_info_path,
+    select_device,
+)
+
+
+def test_decode_c_string_stops_at_first_null() -> None:
+    assert decode_c_string(b"DB2163742\0ignored") == "DB2163742"
+
+
+def test_select_device_accepts_serial_or_guid() -> None:
+    devices = [
+        DeviceDescriptor(
+            index=0,
+            model="MV-CS020-10UM",
+            serial="DB2163742",
+            guid="2BDFB2163742",
+        ),
+        DeviceDescriptor(
+            index=1,
+            model="MV-CS020-10UM",
+            serial="DB2163739",
+            guid="2BDFB2163739",
+        ),
+    ]
+
+    assert select_device(devices, "DB2163742", "MV-CS020-10UM").index == 0
+    assert select_device(devices, "2BDFB2163739", "MV-CS020-10UM").index == 1
+
+
+def test_select_device_never_guesses_when_multiple_cameras_exist() -> None:
+    devices = [
+        DeviceDescriptor(0, "MV-CS020-10UM", "one", "guid-one"),
+        DeviceDescriptor(1, "MV-CS020-10UM", "two", "guid-two"),
+    ]
+
+    with pytest.raises(RuntimeError, match="selector is required"):
+        select_device(devices, "", "MV-CS020-10UM")
+
+
+def test_select_device_rejects_wrong_model() -> None:
+    devices = [DeviceDescriptor(0, "other", "DB2163742", "guid")]
+
+    with pytest.raises(RuntimeError, match="expected a model"):
+        select_device(devices, "DB2163742", "MV-CS020-10UM")
+
+
+def test_load_standard_camera_calibration(tmp_path: Path) -> None:
+    path = tmp_path / "camera.yaml"
+    path.write_text(
+        yaml.safe_dump(
+            {
+                "image_width": 1624,
+                "image_height": 1240,
+                "camera_name": "hikrobot_front_DB2163742",
+                "camera_matrix": {
+                    "rows": 3,
+                    "cols": 3,
+                    "data": [1000.0, 0.0, 812.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 1.0],
+                },
+                "distortion_model": "plumb_bob",
+                "distortion_coefficients": {
+                    "rows": 1,
+                    "cols": 5,
+                    "data": [0.1, -0.2, 0.0, 0.0, 0.1],
+                },
+                "rectification_matrix": {
+                    "rows": 3,
+                    "cols": 3,
+                    "data": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+                },
+                "projection_matrix": {
+                    "rows": 3,
+                    "cols": 4,
+                    "data": [1000.0, 0.0, 812.0, 0.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 0.0, 1.0, 0.0],
+                },
+            },
+            sort_keys=False,
+        ),
+        encoding="utf-8",
+    )
+
+    calibration = load_camera_calibration(path)
+
+    assert calibration.width == 1624
+    assert calibration.height == 1240
+    assert calibration.k[0] == 1000.0
+    assert calibration.p[5] == 1001.0
+
+
+def test_camera_info_url_only_accepts_local_files(tmp_path: Path) -> None:
+    path = resolve_camera_info_path(str(tmp_path / "front.yaml"))
+    assert path == (tmp_path / "front.yaml").resolve()
+    assert resolve_camera_info_path("") is None
+    with pytest.raises(ValueError, match="filesystem path"):
+        resolve_camera_info_path("package://example/front.yaml")

src/g20_thumb_apriltag_calibration/test/test_o30_support.py

@@ -0,0 +1,263 @@
+import math
+from pathlib import Path
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.full_hand import (
+    O30_COMMAND_NAMES,
+    O30_RIGHT_BASELINE_COMMAND,
+    O30_RIGHT_HAND_PROFILE,
+    JointCurveFit,
+    build_calibration_motion_command,
+    build_calibration_speed_profile,
+    build_compact_payload,
+    get_hand_calibration_profile,
+    validate_compact_payload,
+)
+from g20_thumb_apriltag_calibration.urdf_zero import (
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    get_zero_calibration_profile,
+    solve_urdf_zero_offsets,
+    write_zero_corrected_urdf,
+)
+
+
+O30_SOURCE_URDF = Path(
+    "/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/"
+    "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
+)
+
+
+def _curve(zero_command: int, travel_rad: float = 0.8) -> JointCurveFit:
+    values = np.asarray(
+        [travel_rad * (command - zero_command) / 255.0 for command in range(256)]
+    )
+    data = tuple(float(value) for value in values)
+    return JointCurveFit(
+        angle_rad=data,
+        decreasing_rad=data,
+        increasing_rad=data,
+        circle={},
+        maximum_monotonic_correction_rad=0.0,
+        maximum_hysteresis_rad=0.0,
+        quality={},
+    )
+
+
+def test_o30_right_profile_matches_sdk_and_requested_baseline() -> None:
+    profile = get_hand_calibration_profile("right", "O30")
+
+    assert profile is O30_RIGHT_HAND_PROFILE
+    assert profile.command_names == O30_COMMAND_NAMES
+    assert profile.baseline_command == (
+        0, 0, 255, 205, 165, 20,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    )
+    assert O30_RIGHT_BASELINE_COMMAND == profile.baseline_command
+    assert len(profile.active_joints) == 20
+    assert profile.passive_joints == ()
+    assert set(profile.measured_joints) == {
+        "thumb_cmc_roll",
+        "thumb_cmc_yaw",
+        "thumb_mcp",
+        "thumb_ip",
+        "pinky_mcp_roll",
+        "pinky_mcp_pitch",
+        "pinky_pip",
+        "pinky_dip",
+    }
+    assert [spec.motor_index for spec in profile.sweep_specs] == [
+        0, 6, 15, 5, 10, 14, 19, 1,
+    ]
+    assert profile.joint_specs["index_mcp_roll"].motor_index == 2
+    assert profile.joint_specs["pinky_mcp_roll"].motor_index == 5
+    assert profile.joint_specs["thumb_mcp"].motor_index == 6
+    assert profile.joint_specs["pinky_mcp_pitch"].motor_index == 10
+    assert profile.joint_specs["pinky_pip"].motor_index == 14
+    assert profile.joint_specs["thumb_ip"].motor_index == 15
+    assert profile.joint_specs["pinky_dip"].motor_index == 19
+
+    with pytest.raises(ValueError, match="only the right hand"):
+        get_hand_calibration_profile("left", "O30")
+
+
+def test_o30_pinky_roll_clearance_and_sdk_speed_broadcast() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 5)
+    command = build_calibration_motion_command(roll, 100, profile=profile)
+
+    assert command[0:6] == [0, 0, 255, 255, 255, 100]
+    assert command[6:] == [0] * 14
+    assert build_calibration_speed_profile(
+        roll,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    ) == [5] * 5
+
+    dip = next(spec for spec in profile.sweep_specs if spec.motor_index == 19)
+    assert build_calibration_speed_profile(
+        dip,
+        normal_speed=15,
+        index_roll_speed=5,
+        index_flex_speed=10,
+        profile=profile,
+    ) == [10] * 5
+
+
+def test_o30_static_zero_policy_and_compact_payload() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    zero = get_zero_calibration_profile("right", "O30")
+
+    assert set(zero.fixed_direct_zero_offsets_rad) == {
+        "thumb_ip",
+        "pinky_mcp_roll",
+        "pinky_dip",
+    }
+    assert "pinky_mcp_pitch" not in zero.fixed_direct_zero_offsets_rad
+    assert "pinky_pip" not in zero.fixed_direct_zero_offsets_rad
+    assert zero.inherited_static_zero_joints == {}
+    assert zero.inherited_zero_joints["index_dip"] == "pinky_dip"
+
+    measured = {
+        name: _curve(
+            profile.baseline_command[profile.joint_specs[name].motor_index]
+        )
+        for name in profile.measured_joints
+    }
+    offsets = {name: 0.0 for name in profile.active_joints}
+    offsets["pinky_mcp_pitch"] = 0.01
+    offsets["pinky_pip"] = -0.02
+    payload = build_compact_payload(
+        serial_number="O30_RIGHT_TEST",
+        measured_fits=measured,
+        urdf_zero_offsets_rad=offsets,
+        validation_errors_rad=[0.001],
+        passed=True,
+        side="right",
+        model="O30",
+    )
+
+    validate_compact_payload(payload)
+    assert payload["model"] == "O30"
+    assert len(payload["joints"]) == 20
+    for name in (
+        "index_mcp_roll",
+        "middle_mcp_roll",
+        "ring_mcp_roll",
+        "pinky_mcp_roll",
+    ):
+        joint = payload["joints"][name]
+        assert joint["angle_rad"][joint["zero_command_u8"]] == pytest.approx(0.0)
+        assert joint["zero_angles"]["urdf_zero_offset_rad"] == 0.0
+    assert payload["joints"]["pinky_mcp_pitch"]["zero_angles"] == {
+        "urdf_zero_offset_rad": 0.01
+    }
+    assert payload["joints"]["index_mcp_pitch"]["zero_angles"] == {
+        "urdf_zero_offset_rad": 0.0
+    }
+
+
+@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
+def test_o30_source_urdf_has_exact_active_joint_set_and_writer(tmp_path) -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    source_bytes = O30_SOURCE_URDF.read_bytes()
+    model = UrdfKinematicModel(O30_SOURCE_URDF)
+
+    assert set(model.joints) == set(profile.active_joints)
+    assert "thumb_cmc_pitch" not in model.joints
+    destination = write_zero_corrected_urdf(
+        source_urdf=O30_SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="O30_RIGHT_TEST",
+        offsets_rad={name: 0.0 for name in profile.active_joints},
+        timestamp="20260814_120000",
+    )
+    assert destination.is_file()
+    assert O30_SOURCE_URDF.read_bytes() == source_bytes
+    assert set(UrdfKinematicModel(destination).joints) == set(profile.active_joints)
+
+
+@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
+def test_o30_zero_solver_recovers_only_observable_offsets() -> None:
+    profile = O30_RIGHT_HAND_PROFILE
+    zero = get_zero_calibration_profile("right", "O30")
+    curves = {
+        name: _curve(
+            profile.baseline_command[profile.joint_specs[name].motor_index],
+            math.radians(55.0),
+        )
+        for name in profile.measured_joints
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in profile.joint_specs.items()
+    }
+    expected_degrees = {
+        "thumb_cmc_roll": 2.0,
+        "thumb_cmc_yaw": -3.0,
+        "thumb_mcp": 4.0,
+        "thumb_ip": 0.0,
+        "pinky_mcp_roll": 0.0,
+        "pinky_mcp_pitch": 1.2,
+        "pinky_pip": -1.0,
+        "pinky_dip": 0.0,
+    }
+    offsets = {
+        name: math.radians(expected_degrees[name])
+        for name in zero.direct_zero_joints
+    }
+    model = UrdfKinematicModel(O30_SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
+    base_translation = np.asarray([0.25, -0.12, 0.68])
+    measurements = []
+    for cycle in range(3):
+        for joint in zero.axis_joints:
+            state = tuple(float(value) for value in profile.baseline_command)
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            axis, point = model.axis_line(
+                joint,
+                zero_offsets=offsets,
+                joint_angles=angles,
+            )
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=state,
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                    pose_axis_line_rms_m=0.0001,
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=O30_SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type="right",
+        hand_model="O30",
+    )
+
+    assert result.passed is True
+    for name, expected in expected_degrees.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            expected, abs=0.05
+        )
+    assert result.all_active_offsets_rad["index_mcp_roll"] == 0.0
+    assert result.all_active_offsets_rad["index_mcp_pitch"] == 0.0

src/g20_thumb_apriltag_calibration/test/test_offline_replay.py

@@ -0,0 +1,44 @@
+import pytest
+
+from g20_thumb_apriltag_calibration.offline_replay import (
+    _latest_attempt_records,
+    _output_suffix,
+)
+
+
+def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
+    return {
+        "kind": "sample",
+        "joint": joint,
+        "cycle": cycle,
+        "direction": direction,
+        "attempt": attempt,
+    }
+
+
+def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
+    rows = [
+        {"kind": "session_start"},
+        _sample("pinky_pip", 0, "decreasing", 1),
+        _sample("pinky_pip", 0, "decreasing", 3),
+        _sample("pinky_pip", 0, "increasing", 1),
+        _sample("pinky_pip", 1, "decreasing", 2),
+        _sample("thumb_cmc_yaw", 0, "decreasing", 1),
+    ]
+
+    selected = _latest_attempt_records(rows)
+
+    assert [
+        record["attempt"] for record in selected["pinky_pip"]
+    ] == [3, 1, 2]
+    assert [
+        record["attempt"] for record in selected["thumb_cmc_yaw"]
+    ] == [1]
+
+
+def test_output_suffix_is_safe_and_explicit() -> None:
+    assert _output_suffix(None) == ""
+    assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
+    for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
+        with pytest.raises(ValueError, match="output tag"):
+            _output_suffix(invalid)

src/g20_thumb_apriltag_calibration/test/test_pnp.py

@@ -0,0 +1,661 @@
+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_group_tracker_initializes_from_multiple_static_frames() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("parent", "child"),
+        adjacent_pairs=(("parent", "child"),),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+    )
+    parent = _pose(0.0, 0.00, 0.05)
+    true_child = _pose(20.0, 0.03, 0.10)
+
+    for index in range(7):
+        false_child = _pose(5.0 * index, 0.03, 0.05)
+        selected, reason = tracker.select(
+            {
+                "parent": (parent,),
+                "child": (false_child, true_child),
+            },
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+        assert selected is None
+        assert reason == f"group_initializing:{index + 1}/8"
+
+    false_child = _pose(35.0, 0.03, 0.05)
+    selected, reason = tracker.select(
+        {
+            "parent": (parent,),
+            "child": (false_child, true_child),
+        },
+        stamp_ns=1_231_000_000,
+    )
+
+    assert reason == ""
+    assert selected == {"parent": parent, "child": true_child}
+    assert (
+        tracker.last_initialization_quality["initialization_search"]
+        == "static_reference"
+    )
+    assert tracker.last_initialization_quality[
+        "p95_pair_rotation_drift_rad"
+    ] < np.deg2rad(1.0)
+
+
+def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("mcp", "pip", "dip"),
+        adjacent_pairs=(("mcp", "pip"), ("pip", "dip")),
+        normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")),
+        normal_alignment_scale_rad=np.deg2rad(5.0),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+        maximum_normal_alignment_rad=np.deg2rad(15.0),
+    )
+    mcp = _pose(12.0, 0.00, 0.05)
+    correct_pip = _pose(14.0, 0.03, 0.10)
+    mirror_pip = _pose(50.0, 0.03, 0.05)
+    dip = _pose(15.0, 0.06, 0.05)
+
+    for index in range(8):
+        selected, reason = tracker.select(
+            {
+                "mcp": (mcp,),
+                "pip": (mirror_pip, correct_pip),
+                "dip": (dip,),
+            },
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+
+    assert reason == ""
+    assert selected == {
+        "mcp": mcp,
+        "pip": correct_pip,
+        "dip": dip,
+    }
+    assert tracker.last_initialization_quality[
+        "maximum_normal_alignment_rad"
+    ] < np.deg2rad(3.0)
+
+
+def test_static_group_rejects_when_no_aligned_branch_exists() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("base", "moving"),
+        adjacent_pairs=(("base", "moving"),),
+        normal_alignment_pairs=(("base", "moving"),),
+        normal_alignment_scale_rad=np.deg2rad(5.0),
+        maximum_normal_alignment_rad=np.deg2rad(15.0),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+        initialization_frames=8,
+    )
+    base = _pose(0.0, 0.00, 0.05)
+    mirror = _pose(38.0, 0.03, 0.05)
+
+    for index in range(8):
+        selected, reason = tracker.select(
+            {"base": (base,), "moving": (mirror,)},
+            stamp_ns=1_000_000_000 + index * 33_000_000,
+        )
+
+    assert selected is None
+    assert reason == "group_normal_alignment"
+
+
+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_three_camera_diagnostics.py

@@ -0,0 +1,406 @@
+import json
+
+import numpy as np
+
+from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
+    _task_text,
+    render_three_camera_status_text_zh,
+    three_camera_reason_zh,
+)
+
+
+def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "sweep_missing_endpoint_bin",
+        "progress": 0.1429,
+        "completed_sweeps": 6,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "sweep",
+            "view": "front",
+            "motor_index": 5,
+            "joints": ["thumb_cmc_roll"],
+            "cycle": 1,
+            "repetitions": 3,
+            "start_u8": 255,
+            "target_u8": 0,
+            "actual_u8": 0.4,
+            "motion_progress": 0.998,
+            "valid_frames": 239,
+            "sample": {
+                "minimum_u8": 0.4,
+                "maximum_u8": 248.2,
+                "bin_count": 180,
+                "minimum_bin_count": 32,
+                "maximum_bin_gap": 3,
+                "allowed_maximum_bin_gap": 16,
+                "missing_endpoint_u8": [255],
+                "endpoint_tolerance_u8": 2.0,
+            },
+        },
+        "views": {
+            "front": {
+                "ready": False,
+                "detection_hz": 30.04,
+                "valid_rate": 0.0,
+                "missing_tag_ids": [2],
+                "pnp_rejections": {},
+                "group_pnp_reason": "group_pose_jump",
+                "pnp_invalid_seconds": 0.8,
+                "pnp_reset_count": 2,
+            },
+            "side": {
+                "ready": True,
+                "detection_hz": 30.02,
+                "valid_rate": 1.0,
+                "missing_tag_ids": [],
+            },
+        },
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "标定已暂停（PAUSED）" in text
+    assert "缺少电机端点255" in text
+    assert "实际电机范围为0.4～248.2" in text
+    assert "第1/3轮，255→0" in text
+    assert "计划扫描6/42个方向，扫描进度14.3%" in text
+    assert "当前缺失Tag=2" in text
+    assert "group_pose_jump" not in text
+    assert "PnP拒绝" not in text
+    assert "自动重置" not in text
+    assert "/g20_calibration/resume" in text
+
+
+def test_preflight_lists_missing_tags_in_chinese() -> None:
+    payload = {
+        "state": "PREFLIGHT",
+        "reason": "waiting_for_three_cameras_tags_and_sdk",
+        "progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 0,
+        "active": {},
+        "views": {
+            "top": {
+                "ready": False,
+                "detection_hz": 30.0,
+                "valid_rate": 0.0,
+                "missing_tag_ids": [9],
+            }
+        },
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "设备和标签预检（PREFLIGHT）" in text
+    assert "当前缺失Tag=9" in text
+    assert "全部必需Tag同时有效率0.0%" in text
+
+
+def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "joint_fit_check_failed",
+        "progress": 18 / 42,
+        "completed_sweeps": 18,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "fit_failure",
+            "view": "front",
+            "motor_index": 6,
+            "joints": ["index_mcp_roll"],
+            "attempt": 1,
+            "directions_to_rescan": 6,
+            "failures": [
+                {
+                    "joint": "index_mcp_roll",
+                    "metric": "arc_deg",
+                    "actual": 3.98,
+                    "limit": 15.0,
+                    "comparison": "minimum",
+                }
+            ],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "食指MCP侧摆的实测圆弧为3.98°" in text
+    assert "要求至少15.00°" in text
+    assert "只清除当前失败关节的数据并重扫6个方向" in text
+    assert "第1次尝试" in text
+    assert "运动采样：" not in text
+
+
+def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "zero_model_validation_failed",
+        "progress": 0.92,
+        "scan_progress": 1.0,
+        "completed_sweeps": 42,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "zero_model_failure",
+            "view": "front",
+            "motor_index": 15,
+            "joints": ["thumb_mcp", "thumb_ip"],
+            "directions_to_rescan": 0,
+            "failures": [
+                {
+                    "joint": "thumb_mcp",
+                    "metric": "zero_guard",
+                    "reason": "zero_offset_exceeds_configured_limit",
+                    "actual_deg": -39.81,
+                    "limit_deg": 20.0,
+                }
+            ],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "拇指MCP：零位估计超过安全范围" in text
+    assert "估计-39.81°，允许±20.00°" in text
+    assert "不会自动重扫" in text
+    assert "总体进度：92.0%" in text
+    assert "扫描进度100.0%" in text
+    assert "运动采样：" not in text
+
+
+def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
+    active = {
+        "kind": "sweep",
+        "view": "front",
+        "motor_index": 15,
+        "joints": ["thumb_mcp", "thumb_ip"],
+        "cycle": 1,
+        "repetitions": 3,
+        "start_u8": 255,
+        "target_u8": 0,
+        "fit_attempt": 2,
+        "fit_attempt_limit": 3,
+    }
+
+    assert "整关节自动重采第2/3次" in _task_text(active)
+
+
+def test_o30_status_shows_cycle_order_retry_execution_and_namespace() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": "joint_fit_check_failed",
+        "progress": 0.112,
+        "scan_progress": 0.125,
+        "completed_sweeps": 6,
+        "total_sweeps": 48,
+        "executed_sweep_directions": 20,
+        "service_prefix": "/o30_calibration",
+        "active": {
+            "kind": "fit_failure",
+            "view": "front",
+            "motor_index": 0,
+            "joints": ["thumb_cmc_roll"],
+            "attempt": 3,
+            "fit_attempt_limit": 3,
+            "directions_to_rescan": 6,
+            "failures": [],
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "已启动20个方向（含自动重扫）" in text
+    assert "重扫不会重复增加计划进度" in text
+    assert "当前为第3/3次采集结果" in text
+    assert "/o30_calibration/resume" in text
+    assert "/g20_calibration/resume" not in text
+
+
+def test_motor_stall_reason_is_explained_in_chinese() -> None:
+    reason, action = three_camera_reason_zh(
+        "PAUSED",
+        "motor_state_stalled:sweep_motor_19:error_u8=5.000",
+        {},
+    )
+
+    assert "距目标5.000个u8" in reason
+    assert "机械端点" in action
+    assert "不要反复调用resume" in action
+
+
+def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": (
+            "motor_state_stalled:return_baseline:motor_index=10:"
+            "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
+            "error_u8=5.000"
+        ),
+        "progress": 0.0,
+        "scan_progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "motion_stall",
+            "stage": "return_baseline",
+            "motor_index": 10,
+            "target_u8": 255.0,
+            "actual_u8": 250.0,
+            "error_u8": 5.0,
+            "tolerance_u8": 4.0,
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "电机10反馈连续8秒" in text
+    assert "目标255.0、实际250.0、误差5.000 u8" in text
+    assert "允许容差±4.0 u8" in text
+    assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text
+    assert "运动采样：" not in text
+
+
+def test_return_baseline_prints_the_exact_command() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    payload = {
+        "state": "RETURN_BASELINE",
+        "reason": "return_baseline_before_next_sweep",
+        "progress": 0.0,
+        "completed_sweeps": 0,
+        "total_sweeps": 42,
+        "baseline_command_u8": baseline,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "正在恢复目标姿态（RETURN_BASELINE）" in text
+    assert f"正在确认基准姿态：{baseline}" in text
+
+
+def test_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None:
+    baseline = [255] * 20
+    baseline[6:10] = [127, 127, 127, 127]
+    recovery = list(baseline)
+    recovery[5] = 145
+    recovery[10] = 0
+    payload = {
+        "state": "RETURN_BASELINE",
+        "reason": "return_baseline_before_retry_sweep",
+        "progress": 0.5,
+        "completed_sweeps": 36,
+        "total_sweeps": 42,
+        "baseline_command_u8": baseline,
+        "return_command_u8": recovery,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert f"正在确认恢复姿态：{recovery}" in text
+
+
+def test_status_numeric_diagnostics_are_json_serializable() -> None:
+    bins = [0, 16, 255]
+    payload = {
+        "state": "SWEEP",
+        "reason": "collecting_timestamp_synchronised_tag_centres",
+        "active": {
+            "sample": {
+                # np.diff返回NumPy标量；节点必须在放入状态前转成原生int。
+                "maximum_bin_gap": int(max(np.diff(bins), default=0)),
+            }
+        },
+    }
+
+    encoded = json.dumps(payload, ensure_ascii=False)
+
+    assert '"maximum_bin_gap": 239' in encoded
+
+
+def test_urdf_zero_bound_has_specific_chinese_scale_guidance() -> None:
+    payload = {
+        "state": "PAUSED",
+        "reason": (
+            "URDF zero offset reached the configured 20.000 degree bound: "
+            "thumb_cmc_roll=+20.000deg, index_pip=-20.000deg; "
+            "all_offsets: thumb_cmc_roll=+20.000deg"
+        ),
+        "progress": 1.0,
+        "completed_sweeps": 42,
+        "total_sweeps": 42,
+        "active": {},
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "触及±20.000°安全边界" in text
+    assert "拇指CMC滚转=+20.000deg" in text
+    assert "食指PIP=-20.000deg" in text
+    assert "不要调用resume" in text
+    assert "Tag有效黑框边长" in text
+    assert "未分类原因码" not in text
+
+
+def test_index_roll_status_prints_clearance_motor_feedback() -> None:
+    payload = {
+        "state": "SWEEP",
+        "reason": "collecting_timestamp_synchronised_tag_centres",
+        "progress": 0.43,
+        "completed_sweeps": 18,
+        "total_sweeps": 42,
+        "active": {
+            "kind": "sweep",
+            "view": "front",
+            "motor_index": 6,
+            "joints": ["index_mcp_roll"],
+            "cycle": 1,
+            "repetitions": 3,
+            "automatic_retry_count": 1,
+            "automatic_retry_limit": 2,
+            "start_u8": 255,
+            "target_u8": 0,
+            "actual_u8": 44.0,
+            "motion_progress": 0.827,
+            "valid_frames": 971,
+            "sample": {"minimum_u8": 40.0, "maximum_u8": 253.0},
+            "auxiliary_motors": [
+                {"motor_index": 7, "command_u8": 0, "actual_u8": 0.0},
+                {"motor_index": 8, "command_u8": 0, "actual_u8": 1.0},
+                {"motor_index": 9, "command_u8": 0, "actual_u8": 0.0},
+            ],
+            "speed": {
+                "commanded_finger_speed": [15, 5, 15, 15, 15],
+                "reported_finger_speed": [15, 5, 15, 15, 15],
+            },
+        },
+        "views": {},
+        "result_path": "",
+    }
+
+    text = render_three_camera_status_text_zh(payload)
+
+    assert "避挡姿态：电机7目标0、实际0.0" in text
+    assert "电机8目标0、实际1.0" in text
+    assert "电机9目标0、实际0.0" in text
+    assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
+    assert "SDK报告[15, 5, 15, 15, 15]" in text
+    assert "自动重试：当前方向已自动重扫1/2次" in text

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_urdf_zero.py

@@ -0,0 +1,977 @@
+import math
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.extrinsics import (
+    camera_info_fingerprint,
+    dump_three_camera_extrinsics,
+    load_three_camera_extrinsics,
+)
+from g20_thumb_apriltag_calibration.urdf_zero import (
+    AXIS_JOINTS,
+    DIRECT_ZERO_JOINTS,
+    INHERITED_ZERO_JOINTS,
+    JointAxisMeasurement,
+    UrdfKinematicModel,
+    _angles_from_state,
+    _zero_sensitive_axis_error_rad,
+    fit_joint_axis_measurement,
+    fit_rotation_joint_curve,
+    solve_urdf_zero_offsets,
+    get_zero_calibration_profile,
+    write_zero_corrected_urdf,
+)
+from g20_thumb_apriltag_calibration.full_hand import (
+    ACTIVE_JOINTS,
+    JOINT_SPECS,
+    MEASURED_JOINTS,
+    PASSIVE_JOINTS,
+    JointCurveFit,
+    get_hand_calibration_profile,
+)
+
+
+REPOSITORY = Path(__file__).resolve().parents[3]
+SOURCE_URDF = REPOSITORY / (
+    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+    "linker_hand/g20_left/linkerhand_g20_left.urdf"
+)
+RIGHT_SOURCE_URDF = REPOSITORY / (
+    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+    "linker_hand/g20_right/linkerhand_g20_right.urdf"
+)
+
+
+def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
+    parent = np.asarray([0.0, 0.0, 1.0])
+    predicted = np.asarray([1.0, 0.0, 0.0])
+    cone_mismatch = np.asarray(
+        [math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
+    )
+    zero_mismatch = np.asarray(
+        [math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
+    )
+
+    assert _zero_sensitive_axis_error_rad(
+        predicted, cone_mismatch, parent
+    ) == pytest.approx(0.0, abs=1.0e-12)
+    assert math.degrees(
+        _zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
+    ) == pytest.approx(3.0, abs=1.0e-9)
+
+
+def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
+    parent = np.asarray([0.0, 0.0, 1.0])
+    cone = math.radians(32.0)
+    phase = math.radians(7.0)
+    predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
+    observed = Rotation.from_rotvec(parent * phase).apply(predicted)
+
+    error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
+
+    assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
+
+
+def _payload(transform: np.ndarray) -> dict[str, list[float]]:
+    return {
+        "translation_xyz_m": transform[:3, 3].tolist(),
+        "quaternion_xyzw": Rotation.from_matrix(
+            transform[:3, :3]
+        ).as_quat().tolist(),
+    }
+
+
+def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
+    axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
+    axis_parent /= np.linalg.norm(axis_parent)
+    centre_parent = np.asarray([0.012, -0.008, 0.021])
+    radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
+    radial = 0.035 * radial / np.linalg.norm(radial)
+    child_tag_mount = Rotation.from_euler(
+        "xyz", [1.1, -0.7, 0.45]
+    )
+    common_from_parent = np.eye(4)
+    common_from_parent[:3, :3] = Rotation.from_euler(
+        "xyz", [-0.8, 0.55, 1.3]
+    ).as_matrix()
+    common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
+    expected_axis = common_from_parent[:3, :3] @ axis_parent
+    expected_point = (
+        common_from_parent[:3, :3] @ centre_parent
+        + common_from_parent[:3, 3]
+    )
+
+    commands = list(range(0, 256, 16)) + [255]
+    records = []
+    for cycle in range(3):
+        for direction in ("decreasing", "increasing"):
+            for command in commands:
+                angle = math.radians(62.0) * (255.0 - command) / 255.0
+                motion = Rotation.from_rotvec(axis_parent * angle)
+                relative_rotation = motion * child_tag_mount
+                relative_translation = centre_parent + motion.apply(radial)
+                child_common = common_from_parent.copy()
+                child_common[:3, :3] = (
+                    common_from_parent[:3, :3]
+                    @ relative_rotation.as_matrix()
+                )
+                child_common[:3, 3] = (
+                    common_from_parent[:3, :3] @ relative_translation
+                    + common_from_parent[:3, 3]
+                )
+                state = [255.0] * 20
+                state[5] = float(command)
+                records.append(
+                    {
+                        "cycle": cycle,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_translation_xyz_m": relative_translation.tolist(),
+                        "relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
+                        "parent_pose_common": _payload(common_from_parent),
+                        "child_pose_common": _payload(child_common),
+                        "state_u8": state,
+                    }
+                )
+    return records, expected_axis, expected_point
+
+
+def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    curve = fit_rotation_joint_curve(records, zero_command_u8=255)
+    measurement = fit_joint_axis_measurement(
+        "thumb_cmc_roll", records, cycle=0, zero_command_u8=255
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+    assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
+    assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
+
+
+@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
+def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
+    joint: str,
+) -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    axis_parent = parent_rotation.inv().apply(expected_axis)
+    tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
+    tangent /= np.linalg.norm(tangent)
+    # Reproduce monocular planar-PnP depth bias: the centre trajectory remains
+    # precise in its dominant directions but receives a command-correlated
+    # component that makes a free 3-D plane normal substantially wrong.
+    biased_records = []
+    for record in records:
+        biased = dict(record)
+        point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
+        depth_bias = 0.30 * float(point @ tangent)
+        biased["relative_translation_xyz_m"] = (
+            point + depth_bias * axis_parent
+        ).tolist()
+        biased_records.append(biased)
+
+    measurement = fit_joint_axis_measurement(
+        joint, biased_records, cycle=0, zero_command_u8=255
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 0.003
+    assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
+    assert measurement.plane_rms_m < 0.003
+    assert measurement.radial_rms_m < 0.003
+
+
+def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    axis_parent = common_from_parent.inv().apply(expected_axis)
+    # Exact end-on depth is a gauge along the physical axis and therefore
+    # cannot alter the observable axis line.  An oblique camera has a small
+    # irreducible coupling between monocular depth and radial position; that
+    # case must be bounded by the residual/holdout gates, not asserted to be
+    # exactly recoverable from one view.
+    view_normal_parent = axis_parent
+    view_normal_common = common_from_parent.apply(view_normal_parent)
+    biased = []
+    for record in records:
+        changed = dict(record)
+        fraction = (255.0 - float(record["command_u8"])) / 255.0
+        depth_bias = 0.03 * (fraction - 0.5)
+        changed["relative_translation_xyz_m"] = (
+            np.asarray(record["relative_translation_xyz_m"], dtype=float)
+            + depth_bias * view_normal_parent
+        ).tolist()
+        biased.append(changed)
+
+    measurement = fit_joint_axis_measurement(
+        "thumb_cmc_pitch",
+        biased,
+        cycle=0,
+        zero_command_u8=255,
+        view_normal_common_xyz=view_normal_common,
+    )
+
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert measurement.axis_point_source == "pose_trajectory_image_plane"
+    assert measurement.pose_axis_line_rms_m < 1.0e-6
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+
+
+def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
+    records, expected_axis, _ = _arbitrary_tag_records()
+    assert not any(record["command_u8"] == 127 for record in records)
+
+    curve = fit_rotation_joint_curve(records, zero_command_u8=127)
+    measurement = fit_joint_axis_measurement(
+        "index_mcp_roll", records, cycle=0, zero_command_u8=127
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
+
+
+def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
+    records, expected_axis, expected_point = _arbitrary_tag_records()
+    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
+    physical_axis_parent = common_from_parent.inv().apply(expected_axis)
+    wrong_axis_parent = np.cross(
+        physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
+    )
+    wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
+    mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
+    contradictory = []
+    for record in records:
+        changed = dict(record)
+        angle = math.radians(62.0) * (
+            255.0 - float(record["command_u8"])
+        ) / 255.0
+        changed["relative_quaternion_xyzw"] = (
+            Rotation.from_rotvec(wrong_axis_parent * angle) * mount
+        ).as_quat().tolist()
+        contradictory.append(changed)
+
+    measurement = fit_joint_axis_measurement(
+        "index_dip",
+        contradictory,
+        cycle=0,
+        zero_command_u8=255,
+        axis_common_constraint=expected_axis,
+    )
+
+    observed_axis = np.asarray(measurement.axis_common_xyz)
+    observed_point = np.asarray(measurement.point_common_xyz_m)
+    assert abs(float(observed_axis @ expected_axis)) > math.cos(
+        math.radians(0.05)
+    )
+    assert np.linalg.norm(
+        np.cross(observed_point - expected_point, expected_axis)
+    ) < 1.0e-6
+
+
+def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
+    cameras = {
+        view: {
+            "serial_number": f"SERIAL_{view}",
+            "width": 1624,
+            "height": 1240,
+            "intrinsics_sha256": camera_info_fingerprint(
+                width=1624,
+                height=1240,
+                camera_matrix=np.asarray(
+                    [[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
+                ),
+            ),
+        }
+        for view in ("front", "side", "top")
+    }
+    transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
+    transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
+    transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
+    transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
+    transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
+    destination = tmp_path / "extrinsics.yaml"
+
+    dump_three_camera_extrinsics(
+        destination,
+        cameras=cameras,
+        front_from_view=transforms,
+        quality={
+            "passed": True,
+            "reprojection_rms_px": 0.3,
+            "maximum_rotation_repeatability_deg": 0.2,
+            "maximum_translation_repeatability_m": 0.001,
+            "front_side_captures": 15,
+            "front_top_captures": 15,
+        },
+    )
+    loaded = load_three_camera_extrinsics(destination)
+
+    assert loaded.cameras["front"].serial_number == "SERIAL_front"
+    assert np.allclose(loaded.transform("side"), transforms["side"])
+    assert np.allclose(loaded.transform("top"), transforms["top"])
+    assert loaded.camera_matches(
+        "front",
+        serial_number="SERIAL_front",
+        width=1624,
+        height=1240,
+        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
+    )
+    assert not loaded.camera_matches(
+        "front",
+        serial_number="WRONG_SERIAL",
+        width=1624,
+        height=1240,
+        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
+    )
+
+
+def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
+    joint = next(
+        element
+        for element in ET.parse(path).getroot().findall("joint")
+        if element.get("name") == name
+    )
+    origin = joint.find("origin")
+    axis = joint.find("axis")
+    xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
+    rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
+    axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
+    return np.block(
+        [
+            [Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
+            [np.asarray([[0.0, 0.0, 0.0, 1.0]])],
+        ]
+    ), axis_xyz / np.linalg.norm(axis_xyz)
+
+
+def _joint_limit(path: Path, name: str) -> tuple[float, float]:
+    joint = next(
+        element
+        for element in ET.parse(path).getroot().findall("joint")
+        if element.get("name") == name
+    )
+    limit = joint.find("limit")
+    return float(limit.get("lower")), float(limit.get("upper"))
+
+
+def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
+    offset = math.radians(7.3)
+    destination = write_zero_corrected_urdf(
+        source_urdf=SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="G20_LEFT_001",
+        offsets_rad={"thumb_cmc_yaw": offset},
+        timestamp="20260806_120000",
+    )
+    original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
+    corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
+    expected = original.copy()
+    expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
+        axis * offset
+    ).as_matrix()
+
+    assert destination != SOURCE_URDF
+    assert np.allclose(corrected, expected, atol=1.0e-12)
+    with pytest.raises(ValueError, match="refusing to overwrite"):
+        write_zero_corrected_urdf(
+            source_urdf=SOURCE_URDF,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": offset},
+            timestamp="20260806_120000",
+        )
+    with pytest.raises(ValueError, match="original CAD URDF"):
+        write_zero_corrected_urdf(
+            source_urdf=destination,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": offset},
+            timestamp="20260806_120001",
+        )
+    with pytest.raises(ValueError, match="finite and within"):
+        write_zero_corrected_urdf(
+            source_urdf=SOURCE_URDF,
+            output_directory=tmp_path,
+            serial_number="G20_LEFT_001",
+            offsets_rad={"thumb_cmc_yaw": math.nan},
+            timestamp="20260806_120002",
+        )
+
+
+def test_urdf_writer_changes_only_the_16_active_zero_origins(
+    tmp_path: Path,
+) -> None:
+    before = SOURCE_URDF.read_bytes()
+    offsets = {
+        name: math.radians(0.25 * (index + 1))
+        for index, name in enumerate(ACTIVE_JOINTS)
+    }
+    destination = write_zero_corrected_urdf(
+        source_urdf=SOURCE_URDF,
+        output_directory=tmp_path,
+        serial_number="G20_LEFT_001",
+        offsets_rad=offsets,
+        timestamp="20260807_180000",
+    )
+
+    assert len(offsets) == 16
+    assert SOURCE_URDF.read_bytes() == before
+    for name in ACTIVE_JOINTS:
+        original, axis = _joint_origin(SOURCE_URDF, name)
+        corrected, corrected_axis = _joint_origin(destination, name)
+        expected = original.copy()
+        expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
+            axis * offsets[name]
+        ).as_matrix()
+        assert np.allclose(corrected, expected, atol=1.0e-12)
+        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
+    for name in PASSIVE_JOINTS:
+        original, axis = _joint_origin(SOURCE_URDF, name)
+        corrected, corrected_axis = _joint_origin(destination, name)
+        assert np.allclose(corrected, original, atol=1.0e-12)
+        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
+    # A zero calibration must not silently expand mechanical/CAD safety
+    # limits.  Dynamic measured ranges remain in the calibration JSON.
+    for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
+        assert _joint_limit(destination, name) == pytest.approx(
+            _joint_limit(SOURCE_URDF, name)
+        )
+
+
+def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
+    values = np.asarray(
+        [travel * (255.0 - command) / 255.0 for command in range(256)]
+    )
+    values -= values[zero_command]
+    data = tuple(float(value) for value in values)
+    return JointCurveFit(
+        angle_rad=data,
+        decreasing_rad=data,
+        increasing_rad=data,
+        circle={},
+        maximum_monotonic_correction_rad=0.0,
+        maximum_hysteresis_rad=0.0,
+        quality={},
+    )
+
+
+def _solve_synthetic_offsets(
+    side: str,
+    offset_degrees: list[float],
+    *,
+    inject_oblique_optical_depth_bias: bool = False,
+    inject_secondary_root_axis_bias_degrees: float = 0.0,
+    inject_secondary_root_point_bias_m: float = 0.0,
+    inject_observer_cone_bias_degrees: float = 0.0,
+    pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
+    joint_maximum_offset_degrees: dict[str, float] | None = None,
+):
+    hand = get_hand_calibration_profile(side)
+    zero = get_zero_calibration_profile(side)
+    source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[hand.joint_specs[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in hand.measured_joints
+    }
+    if inject_secondary_root_axis_bias_degrees:
+        # Make the thumb root the higher-travel, directly observed direction,
+        # matching the real right-hand data where the short pinky splay arc is
+        # the less reliable root-axis orientation estimate.
+        curves["thumb_cmc_roll"] = _synthetic_curve(
+            int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
+            math.radians(70.0),
+        )
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in hand.joint_specs.items()
+    }
+    offsets = {
+        name: math.radians(value)
+        for name, value in zip(zero.direct_zero_joints, offset_degrees)
+    }
+    model = UrdfKinematicModel(source)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements: list[JointAxisMeasurement] = []
+    for cycle in range(3):
+        for joint in zero.axis_joints:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            axis, point = model.axis_line(
+                joint, zero_offsets=offsets, joint_angles=angles
+            )
+            point_common = base_rotation.apply(point) + base_translation
+            if (
+                inject_secondary_root_point_bias_m
+                and joint == f"{zero.reference_finger}_mcp_roll"
+            ):
+                # A repeatable monocular depth error on the second parallel
+                # root line must affect translation only, never palm rotation
+                # or the inferred thumb-roll zero.
+                point_common = point_common + base_rotation.apply(
+                    np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
+                )
+            view_normal_common = None
+            if (
+                inject_oblique_optical_depth_bias
+                and joint in zero.phase_parent_joint
+            ):
+                parent_axis = model.axis_line(
+                    zero.phase_parent_joint[joint],
+                    zero_offsets=offsets,
+                    joint_angles=angles,
+                )[0]
+                helper = (
+                    np.asarray([1.0, 0.0, 0.0])
+                    if abs(float(parent_axis[0])) < 0.8
+                    else np.asarray([0.0, 1.0, 0.0])
+                )
+                tilt_axis = np.cross(parent_axis, helper)
+                tilt_axis /= np.linalg.norm(tilt_axis)
+                view_normal = Rotation.from_rotvec(
+                    math.radians(15.0) * tilt_axis
+                ).apply(parent_axis)
+                view_normal_common = tuple(base_rotation.apply(view_normal))
+                # Simulate an independent planar-PnP depth error on the child
+                # Tag. It is large enough to drive the old 3-D phase solve to
+                # a configured offset bound.
+                point_common = point_common + 0.03 * np.asarray(
+                    view_normal_common
+                )
+            axis_common = base_rotation.apply(axis)
+            if (
+                inject_observer_cone_bias_degrees
+                and joint == "thumb_cmc_pitch"
+            ):
+                parent_axis = model.axis_line(
+                    zero.axis_parent_joint[joint],
+                    zero_offsets=offsets,
+                    joint_angles=angles,
+                )[0]
+                cone_normal = np.cross(axis, parent_axis)
+                cone_normal /= np.linalg.norm(cone_normal)
+                axis_common = base_rotation.apply(
+                    Rotation.from_rotvec(
+                        math.radians(inject_observer_cone_bias_degrees)
+                        * cone_normal
+                    ).apply(axis)
+                )
+            if (
+                inject_secondary_root_axis_bias_degrees
+                and joint == f"{zero.reference_finger}_mcp_roll"
+            ):
+                helper = np.asarray([0.0, 0.0, 1.0])
+                if abs(float(axis_common @ helper)) > 0.8:
+                    helper = np.asarray([0.0, 1.0, 0.0])
+                bias_axis = np.cross(axis_common, helper)
+                bias_axis /= np.linalg.norm(bias_axis)
+                axis_common = Rotation.from_rotvec(
+                    math.radians(inject_secondary_root_axis_bias_degrees)
+                    * bias_axis
+                ).apply(axis_common)
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(axis_common),
+                    point_common_xyz_m=tuple(point_common),
+                    condition_state_u8=tuple(state),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                    view_normal_common_xyz=view_normal_common,
+                    pose_axis_line_rms_m=(
+                        pose_axis_line_rms_by_joint_m or {}
+                    ).get(joint, 0.0),
+                )
+            )
+    result = solve_urdf_zero_offsets(
+        source_urdf=source,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type=side,
+        joint_maximum_offset_rad={
+            name: math.radians(value)
+            for name, value in (joint_maximum_offset_degrees or {}).items()
+        },
+    )
+    return zero, result
+
+
+def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
+    zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, value in result.direct_offsets_rad.items():
+        assert value == pytest.approx(static_policy.get(name, 0.0))
+
+
+def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
+    right = get_zero_calibration_profile("right")
+    left = get_zero_calibration_profile("left")
+
+    assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
+    assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
+    assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
+    assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
+
+
+def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
+    )
+    reference_roll = f"{zero.reference_finger}_mcp_roll"
+    assert result.passed is True
+    assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
+        0.0, abs=1.0e-12
+    )
+
+
+def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_secondary_root_axis_bias_degrees=15.0,
+    )
+
+    assert result.passed is True
+    expected = dict(
+        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
+    )
+    expected.update(
+        {
+            name: math.degrees(value)
+            for name, value in {
+                **zero.fixed_direct_zero_offsets_rad,
+                **zero.static_output_zero_offsets_rad,
+            }.items()
+        }
+    )
+    for name, value in expected.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            value, abs=0.05
+        )
+
+
+def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_secondary_root_point_bias_m=0.02,
+    )
+
+    assert result.passed is True
+    assert math.degrees(
+        result.direct_offsets_rad["thumb_cmc_roll"]
+    ) == pytest.approx(2.0, abs=0.05)
+    assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
+
+
+def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
+    offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
+    _, result = _solve_synthetic_offsets(
+        "right",
+        offsets,
+        joint_maximum_offset_degrees={"thumb_mcp": 45.0},
+    )
+
+    assert result.passed is True
+    assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
+    assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
+    assert "thumb_ip" not in result.validation_error_by_joint_rad
+    for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
+        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
+    for name in ("pinky_mcp_pitch", "pinky_pip"):
+        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
+    assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
+
+
+def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
+    zero, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_oblique_optical_depth_bias=True,
+    )
+
+    assert result.passed is True
+    expected = dict(
+        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
+    )
+    expected.update(
+        {
+            name: math.degrees(value)
+            for name, value in {
+                **zero.fixed_direct_zero_offsets_rad,
+                **zero.static_output_zero_offsets_rad,
+            }.items()
+        }
+    )
+    for name, value in expected.items():
+        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
+            value, abs=0.05
+        )
+
+
+def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
+    _, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        inject_observer_cone_bias_degrees=8.0,
+    )
+
+    assert result.passed is False
+    assert result.failure_reasons["thumb_cmc_yaw"] == (
+        "zero_axis_cone_mismatch_too_large"
+    )
+
+
+def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
+    _, result = _solve_synthetic_offsets(
+        "right",
+        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
+    )
+
+    assert result.passed is False
+    assert result.failure_reasons["thumb_cmc_pitch"] == (
+        "zero_phase_axis_line_residual_too_large"
+    )
+
+
+def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
+    zero = get_zero_calibration_profile("left")
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[JOINT_SPECS[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in MEASURED_JOINTS
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in JOINT_SPECS.items()
+    }
+    true_offsets = {
+        name: math.radians(value)
+        for name, value in zip(
+            DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
+        )
+    }
+    model = UrdfKinematicModel(SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements = []
+    yaw_axis_without_clearance = None
+    yaw_axis_with_clearance = None
+    for cycle in range(3):
+        for joint in AXIS_JOINTS:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            angles = _angles_from_state(
+                state, curves=curves, motor_by_joint=motor_by_joint
+            )
+            axis, point = model.axis_line(
+                joint,
+                zero_offsets=true_offsets,
+                joint_angles=angles,
+            )
+            if joint == "thumb_cmc_yaw":
+                yaw_axis_with_clearance = axis.copy()
+                baseline_angles = _angles_from_state(
+                    baseline, curves=curves, motor_by_joint=motor_by_joint
+                )
+                yaw_axis_without_clearance = model.axis_line(
+                    joint,
+                    zero_offsets=true_offsets,
+                    joint_angles=baseline_angles,
+                )[0]
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=tuple(state),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+    )
+
+    assert math.degrees(
+        math.acos(
+            np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
+        )
+    ) > 1.0
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, expected in true_offsets.items():
+        if name in static_policy:
+            expected = static_policy[name]
+        assert result.direct_offsets_rad[name] == pytest.approx(
+            expected, abs=math.radians(0.05)
+        )
+    assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
+    assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
+        0.0, abs=math.radians(0.05)
+    )
+    for target in INHERITED_ZERO_JOINTS:
+        assert result.all_active_offsets_rad[target] == pytest.approx(
+            0.0, abs=1.0e-12
+        )
+    assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
+
+
+def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
+    hand = get_hand_calibration_profile("right")
+    zero = get_zero_calibration_profile("right")
+    baseline = [255.0] * 20
+    baseline[6:10] = [127.0] * 4
+    curves = {
+        name: _synthetic_curve(
+            int(baseline[hand.joint_specs[name].motor_index]),
+            math.radians(50.0),
+        )
+        for name in hand.measured_joints
+    }
+    motor_by_joint = {
+        name: spec.motor_index for name, spec in hand.joint_specs.items()
+    }
+    true_offsets = {
+        name: math.radians(value)
+        for name, value in zip(
+            zero.direct_zero_joints,
+            [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
+        )
+    }
+    model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
+    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
+    base_translation = np.asarray([0.31, -0.19, 0.72])
+    measurements: list[JointAxisMeasurement] = []
+    for cycle in range(3):
+        states: dict[str, list[float]] = {}
+        lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
+        for joint in zero.axis_joints:
+            state = list(baseline)
+            if joint == "thumb_cmc_yaw":
+                state[5] = 145.0
+            states[joint] = state
+            angles = _angles_from_state(
+                state,
+                curves=curves,
+                motor_by_joint=motor_by_joint,
+                inherited_zero_joints=zero.inherited_zero_joints,
+            )
+            lines[joint] = model.axis_line(
+                joint,
+                zero_offsets=true_offsets,
+                joint_angles=angles,
+            )
+        original_lines = {
+            name: (axis.copy(), point.copy())
+            for name, (axis, point) in lines.items()
+        }
+        for observer, parent in zero.phase_parent_joint.items():
+            parent_axis, parent_point = lines[parent]
+            original_parent_axis, original_parent_point = original_lines[parent]
+            child_axis, original_child_point = original_lines[observer]
+            radial = original_child_point - original_parent_point
+            radial -= original_parent_axis * float(
+                radial @ original_parent_axis
+            )
+            # Preserve angular phase while deliberately corrupting link radius
+            # and along-axis depth. These components must not move a zero.
+            lines[observer] = (
+                child_axis,
+                parent_point + 1.25 * radial + 0.02 * parent_axis,
+            )
+        for joint in zero.axis_joints:
+            axis, point = lines[joint]
+            measurements.append(
+                JointAxisMeasurement(
+                    joint=joint,
+                    cycle=cycle,
+                    axis_common_xyz=tuple(base_rotation.apply(axis)),
+                    point_common_xyz_m=tuple(
+                        base_rotation.apply(point) + base_translation
+                    ),
+                    condition_state_u8=tuple(states[joint]),
+                    plane_rms_m=0.0002,
+                    radial_rms_m=0.0002,
+                    rotation_circle_axis_difference_rad=math.radians(0.1),
+                )
+            )
+
+    result = solve_urdf_zero_offsets(
+        source_urdf=RIGHT_SOURCE_URDF,
+        measurements=measurements,
+        curves=curves,
+        motor_by_joint=motor_by_joint,
+        hand_type="right",
+    )
+
+    assert result.passed is True
+    static_policy = {
+        **zero.fixed_direct_zero_offsets_rad,
+        **zero.static_output_zero_offsets_rad,
+    }
+    for name, expected in true_offsets.items():
+        if name in static_policy:
+            expected = static_policy[name]
+        assert result.direct_offsets_rad[name] == pytest.approx(
+            expected, abs=math.radians(0.05)
+        )
+    for target in zero.inherited_zero_joints:
+        assert result.all_active_offsets_rad[target] == pytest.approx(
+            0.0, abs=1.0e-12
+        )
+    assert set(result.all_active_offsets_rad) == set(hand.active_joints)

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/config/constants.py

@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
             "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
-            "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
-            "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
-            "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
-            "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+            "OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
+            "拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
+            "拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
+            "拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
             "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
             "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],

src/gui_control/gui_control/gui_control.py

@@ -19,6 +19,33 @@ from .utils.mapping import *
 
 from .config.constants import _HAND_CONFIGS
 LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+
+_CANONICAL_COMMAND_NAMES = {
+    "G20": [
+        "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",
+    ],
+    "O6": [
+        "thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
+        "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    ],
+}
+
+_CANONICAL_COMMAND_BOUNDS = {
+    "G20": [
+        *[(0, 255)] * 10,
+        (0, 255),
+        *[(255, 255)] * 4,
+        *[(0, 255)] * 5,
+    ],
+    "O6": [(0, 255)] * 6,
+}
+
+
 class ROS2NodeManager(QObject):
     """ROS2节点管理器，处理ROS通信"""
     status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
@@ -45,28 +72,43 @@ class ROS2NodeManager(QObject):
             self.node.declare_parameter('hand_joint', 'L10')
             self.node.declare_parameter('topic_hz', 30)
             self.node.declare_parameter('is_arc', False)
+            self.node.declare_parameter('topic_prefix', '')
             
             # 获取参数
             self.hand_type = self.node.get_parameter('hand_type').value
             self.hand_joint = self.node.get_parameter('hand_joint').value
             self.hz = self.node.get_parameter('topic_hz').value
             self.is_arc = self.node.get_parameter('is_arc').value
+            self.topic_prefix = self.normalize_topic_prefix(
+                self.node.get_parameter('topic_prefix').value
+            )
             
             if self.is_arc == True:
                 # 创建发布者
                 self.publisher_arc = self.node.create_publisher(
-                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                    JointState,
+                    self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
+                    10
                 )
             # 创建发布者
             self.publisher = self.node.create_publisher(
-                JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+                JointState,
+                self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
+                10
+            )
+            self.snapshot_publisher = self.node.create_publisher(
+                JointState, self.topic('/calibration_pose_snapshot'), 10
             )
                     # 新增 speed / torque 发布者
             self.speed_pub = self.node.create_publisher(
-                String, f'/cb_hand_setting_cmd', 10)
+                String, self.topic('/cb_hand_setting_cmd'), 10)
             self.torque_pub = self.node.create_publisher(
-                String, f'/cb_hand_setting_cmd', 10)
-            self.status_updated.emit("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+                String, self.topic('/cb_hand_setting_cmd'), 10)
+            self.status_updated.emit(
+                "info",
+                f"ROS2节点初始化成功: {self.topic_prefix or '/'} "
+                f"{self.hand_type} {self.hand_joint}"
+            )
             
             # 启动ROS2自旋线程
             self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
@@ -75,11 +117,34 @@ class ROS2NodeManager(QObject):
             self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}")
             raise
 
+    @staticmethod
+    def normalize_topic_prefix(prefix: str) -> str:
+        prefix = str(prefix).strip()
+        if not prefix or prefix == '/':
+            return ''
+        if not prefix.startswith('/'):
+            prefix = '/' + prefix
+        return prefix.rstrip('/')
+
+    def topic(self, absolute_topic: str) -> str:
+        if not absolute_topic.startswith('/'):
+            raise ValueError('base topic must be absolute')
+        return self.topic_prefix + absolute_topic
+
     def spin_node(self):
         """运行ROS2节点自旋循环"""
         while rclpy.ok() and self.node:
             rclpy.spin_once(self.node, timeout_sec=0.1)
 
+    def bound_positions(self, positions: List[int]) -> List[int]:
+        bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+        if not bounds or len(bounds) != len(positions):
+            return list(positions)
+        return [
+            max(minimum, min(maximum, int(value)))
+            for value, (minimum, maximum) in zip(positions, bounds)
+        ]
+
     def publish_joint_state(self, positions: List[int]):
         """发布关节状态消息"""
         if not self.publisher or not self.node:
@@ -87,6 +152,7 @@ class ROS2NodeManager(QObject):
             return
             
         try:
+            positions = self.bound_positions(positions)
             self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
             self.joint_state.position = [float(pos) for pos in positions]
             # self.joint_state.velocity = [0.1] * len(positions)
@@ -94,7 +160,10 @@ class ROS2NodeManager(QObject):
             # 如果有关节名称，添加到消息中
             #hand_config = HandConfig.from_hand_type(self.hand_joint)
             hand_config = _HAND_CONFIGS[self.hand_joint]
-            if len(hand_config.joint_names) == len(positions):
+            canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
+            if canonical_names and len(canonical_names) == len(positions):
+                self.joint_state.name = canonical_names
+            elif len(hand_config.joint_names) == len(positions):
                 if hand_config.joint_names_en != None:
                     self.joint_state.name = hand_config.joint_names_en
                 else:
@@ -131,9 +200,20 @@ class ROS2NodeManager(QObject):
         except Exception as e:
             self.status_updated.emit("error", f"发布失败: {str(e)}")
 
+    def publish_pose_snapshot(self, positions: List[int]):
+        """发布带名称的当前标定姿势快照。"""
+        positions = self.bound_positions(positions)
+        self.publish_joint_state(positions)
+        self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+        self.joint_state.position = [float(pos) for pos in positions]
+        canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
+        if canonical_names:
+            self.joint_state.name = canonical_names
+        self.snapshot_publisher.publish(self.joint_state)
+        self.status_updated.emit("info", "当前标定姿势快照已发布")
+
     def publish_speed(self, val: int):
-        joint_len = 0
-        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+        if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
             joint_len = 7
@@ -152,8 +232,7 @@ class ROS2NodeManager(QObject):
         self.speed_pub.publish(msg)
 
     def publish_torque(self, val: int):
-        joint_len = 0
-        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+        if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
             joint_len = 7
@@ -212,7 +291,10 @@ class HandControlGUI(QWidget):
     def init_ui(self):
         """初始化用户界面"""
         # 设置窗口属性
-        self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.setWindowTitle(
+            f'灵巧手控制界面 - {self.ros_manager.topic_prefix or "/"} '
+            f'{self.hand_type} {self.hand_joint}'
+        )
         self.setMinimumSize(1200, 900)
         
         # 设置样式
@@ -383,13 +465,18 @@ class HandControlGUI(QWidget):
         for i, (name, value) in enumerate(zip(
             self.hand_config.joint_names, self.hand_config.init_pos
         )):
+            bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+            minimum, maximum = (
+                bounds[i] if bounds and i < len(bounds) else (0, 255)
+            )
+            value = max(minimum, min(maximum, int(value)))
             # 创建标签
             label = QLabel(f"{name}: {value}")
             label.setMinimumWidth(120)
             
             # 创建滑动条
             slider = QSlider(Qt.Horizontal)
-            slider.setRange(0, 255)
+            slider.setRange(minimum, maximum)
             slider.setValue(value)
             slider.valueChanged.connect(
                 lambda val, idx=i: self.on_slider_value_changed(idx, val)
@@ -435,6 +522,11 @@ class HandControlGUI(QWidget):
         self.stop_button.setProperty("category", "danger")
         self.stop_button.clicked.connect(self.on_stop_clicked)
         actions_layout.addWidget(self.stop_button)
+
+        self.save_pose_button = QPushButton("保存当前标定姿势")
+        self.save_pose_button.setProperty("category", "action")
+        self.save_pose_button.clicked.connect(self.on_save_pose_clicked)
+        actions_layout.addWidget(self.save_pose_button)
         
         layout.addLayout(actions_layout)
         
@@ -652,6 +744,11 @@ class HandControlGUI(QWidget):
             
         self.status_updated.emit("warning", "已停止所有动作")
 
+    def on_save_pose_clicked(self):
+        """发布当前滑块姿势，供hand_pose_capture写入profile。"""
+        positions = [slider.value() for slider in self.sliders]
+        self.ros_manager.publish_pose_snapshot(positions)
+
     def on_cycle_clicked(self):
         """循环运行预设动作按钮点击事件处理"""
         if not self.hand_config.preset_actions:

src/linker_hand_ros2_sdk/launch/test.bak

@@ -1,31 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_left',
-            output='screen',
-            parameters=[{
-                'hand_type': 'left',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_right',
-            output='screen',
-            parameters=[{
-                'hand_type': 'right',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-    ])

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py

@@ -1030,6 +1030,16 @@ class LinkerHandG20Can:
         cmd_state = self.joint_state_to_cmd_state(state=s)
         return cmd_state
 
+    def get_cached_current_status(self):
+        """Return the latest received five-finger state without CAN queries."""
+        state = [self.x41, self.x42, self.x43, self.x44, self.x45]
+        if not all(
+            isinstance(finger, (list, tuple)) and len(finger) == 6
+            for finger in state
+        ):
+            return None
+        return self.joint_state_to_cmd_state(state=state)
+
     def get_current_pub_status(self):
         """API接口:获取手指当前状态"""
         self.get_current_status()
@@ -1266,4 +1276,25 @@ class LinkerHandG20Can:
         except:
             return "-1"
     def get_finger_order(self):
-        return ["Thumb Base", "Index Finger Base", "Middle Finger Base", "Ring Finger Base", "Pinky Finger Base", "Thumb Abduction", "Index Finger Abduction", "Middle Finger Abduction", "Ring Finger Abduction", "Pinky Finger Abduction", "Thumb Horizontal Abduction", "Reserved", "Reserved", "Reserved", "Reserved", "Thumb Tip", "Index Finger Tip", "Middle Finger Tip", "Ring Finger Tip", "Pinky Finger Tip"]
+        return [
+            "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",
+        ]

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak

@@ -1,444 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-from pymodbus.client import ModbusSerialClient
-from typing import List, Dict
-import numpy as np
-
-_INTERVAL = 0.006  # 8 ms
-
-class LinkerHandL6RS485:
-    """L6机械手 Modbus-RTU 控制类"""
-    
-    # 6个关节名称
-    JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", 
-                   "middle_pitch", "ring_pitch", "little_pitch"]
-    
-    # 手指名称
-    FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
-    
-    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
-        """
-        初始化L6机械手
-        hand_id: 右手0x27(39), 左手0x28(40)
-        modbus_port: 串口设备路径
-        baudrate: 波特率，固定115200
-        """
-        self.slave = hand_id
-        self.cli = ModbusSerialClient(
-            port=modbus_port, 
-            baudrate=baudrate,
-            bytesize=8, 
-            parity="N", 
-            stopbits=1,
-            timeout=0.05
-        )
-        # pymodbus 3.5.1 需要显式连接
-        self.connected = self.cli.connect()
-        if not self.connected:
-            raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
-
-    def _read_input_registers(self, address: int, count: int) -> List[int]:
-        """读取输入寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
-        return result.registers
-
-    def _write_register(self, address: int, value: int):
-        """写入单个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_register(address=address, value=value, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
-
-    def _write_registers(self, address: int, values: List[int]):
-        """写入多个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_registers(address=address, values=values, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
-
-    # --------------------------------------------------
-    # 基础读取接口
-    # --------------------------------------------------
-    
-    def read_angles(self) -> List[int]:
-        """读取6个关节角度 (输入寄存器 0-5)"""
-        return self._read_input_registers(0, 6)
-
-    def read_torques(self) -> List[int]:
-        """读取6个关节转矩 (输入寄存器 6-11)"""
-        return self._read_input_registers(6, 6)
-
-    def read_speeds(self) -> List[int]:
-        """读取6个关节速度 (输入寄存器 12-17)"""
-        return self._read_input_registers(12, 6)
-
-    def read_temperatures(self) -> List[int]:
-        """读取6个关节温度 (输入寄存器 18-23)"""
-        return self._read_input_registers(18, 6)
-
-    def read_error_codes(self) -> List[int]:
-        """读取6个关节错误码 (输入寄存器 24-29)"""
-        return self._read_input_registers(24, 6)
-
-    # --------------------------------------------------
-    # 压力传感器接口
-    # --------------------------------------------------
-    
-    # def _pressure(self, finger: int) -> List[int]:
-    #     """内部：选手指 → 读压力数据"""
-    #     # 选择手指 (保持寄存器 36)
-    #     self._write_register(36, finger)
-    #     time.sleep(_INTERVAL)
-    #     # 读取压力数据 (输入寄存器 52-122)
-    #     return np.array(self._read_input_registers(52, 71))
-    def _pressure(self, finger: int) -> np.ndarray:
-        """
-        6x12 (72点) 矩阵尺寸。
-        Modbus 地址 60/62。
-        """
-        rows = 12  # 12 行
-        cols = 6   # 6 列
-        finger_size = rows * cols  # 72 个数据点
-        
-        # modbus 地址和计数
-        write_address = 60  # 写入手指选择
-        read_address = 62   # 读取压力数据
-        read_count = 96     # 读取 96 个寄存器
-        skip_count = 10     # 跳过前 10 个校验点
-        
-        # 0. 参数校验和手指写入值确定
-        if finger < 1 or finger > 5:
-            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
-            
-        finger_write_value = finger 
-        
-        # 1. 写入手指选择寄存器 (地址 60)
-        time.sleep(0.008)
-        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
-        if wrsp.isError():
-            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
-
-        # 写入后等待片刻
-        time.sleep(0.008) 
-        
-        # 2. 读取地址 62 的数据
-        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
-        
-        if rrsp.isError():
-            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
-            
-        registers_16bit: List[int] = rrsp.registers 
-        
-        # 3. 核心数据处理
-        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
-        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
-        
-        # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
-        effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
-        # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
-        start_idx = 0 
-        end_idx = finger_size  # 72
-        
-        finger_data_flat = effective_data[start_idx:end_idx]
-        
-        # d. 验证数据长度
-        if finger_data_flat.size != finger_size:
-            raise ValueError(
-                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
-                f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
-            )
-            
-        # e. 重塑为二维矩阵 (12 行 6 列)
-        finger_matrix = finger_data_flat.reshape((rows, cols))
-                
-        return finger_matrix
-
-    def read_pressure_thumb(self) -> np.ndarray:
-        """读取大拇指压力数据"""
-        return np.array(self._pressure(1), dtype=np.uint8)
-
-    def read_pressure_index(self) -> np.ndarray:
-        """读取食指压力数据"""
-        return np.array(self._pressure(2), dtype=np.uint8)
-
-    def read_pressure_middle(self) -> np.ndarray:
-        """读取中指压力数据"""
-        return np.array(self._pressure(3), dtype=np.uint8)
-
-    def read_pressure_ring(self) -> np.ndarray:
-        """读取无名指压力数据"""
-        return np.array(self._pressure(4), dtype=np.uint8)
-
-    def read_pressure_little(self) -> np.ndarray:
-        """读取小拇指压力数据"""
-        return np.array(self._pressure(5), dtype=np.uint8)
-
-    # --------------------------------------------------
-    # 版本信息接口
-    # --------------------------------------------------
-    
-    def read_versions(self) -> Dict[str, int]:
-        """读取版本信息 (输入寄存器 148-155)"""
-        result = self._read_input_registers(148, 8)
-        
-        return {
-            "hand_freedom": result[0],
-            "hand_version": result[1],
-            "hand_number": result[2],
-            "hand_direction": result[3],
-            "software_version_major": result[4],
-            "software_version_minor": result[5] if len(result) > 5 else 0,
-            "software_version_revision": result[6] if len(result) > 6 else 0,
-            "hardware_version": result[7] if len(result) > 7 else 0
-        }
-
-    # --------------------------------------------------
-    # 写入接口
-    # --------------------------------------------------
-    
-    def write_angles(self, vals: List[int]):
-        """设置6个关节角度 (保持寄存器 0-5)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(0, vals)
-
-    def write_torques(self, vals: List[int]):
-        """设置6个关节转矩 (保持寄存器 6-11)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(6, vals)
-
-    def write_speeds(self, vals: List[int]):
-        """设置6个关节速度 (保持寄存器 12-17)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(12, vals)
-
-    # --------------------------------------------------
-    # 上下文管理
-    # --------------------------------------------------
-    
-    def close(self):
-        """关闭连接"""
-        if self.connected:
-            self.cli.close()
-            self.connected = False
-    
-    def __enter__(self):
-        return self
-    
-    def __exit__(self, exc_type, exc_val, exc_tb):
-        self.close()
-
-    # --------------------------------------------------
-    # API固定接口函数
-    # --------------------------------------------------
-    
-    def is_valid_6xuint8(self, lst) -> bool:
-        """验证6个0-255的整数列表"""
-        if len(lst) != 6:
-            return False
-        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
-    
-    def set_joint_positions(self, joint_angles=None):
-        """设置关节位置"""
-        joint_angles = joint_angles or [0] * 6
-        self.write_angles(joint_angles)
-
-    def set_speed(self, speed=None):
-        """设置速度"""
-        speed = speed or [200] * 6
-        self.write_speeds(speed)
-    
-    def set_torque(self, torque=None):
-        """设置扭矩"""
-        torque = torque or [200] * 6
-        self.write_torques(torque)
-
-    def set_current(self, current=None):
-        """设置电流 (L6不支持)"""
-        print("当前L6不支持设置电流", flush=True)
-
-    def get_version(self) -> list:
-        """获取版本信息"""
-        versions = self.read_versions()
-        return [
-            versions.get("hand_freedom", 0),
-            versions.get("hand_version", 0),
-            versions.get("hand_number", 0),
-            versions.get("hand_direction", 0),
-            versions.get("software_version_major", 0),
-            versions.get("hardware_version", 0)
-        ]
-
-    def get_current(self):
-        """获取电流 (L6不支持)"""
-        print("当前L6不支持获取电流", flush=True)
-        return []
-
-    def get_state(self) -> list:
-        """获取关节状态"""
-        return self.read_angles()
-    
-    def get_state_for_pub(self) -> list:
-        return self.get_state()
-
-    def get_current_status(self) -> list:
-        return self.get_state()
-    
-    def get_speed(self) -> list:
-        """获取当前速度"""
-        return self.read_speeds()
-    
-    def get_joint_speed(self) -> list:
-        return self.get_speed()
-    
-    def get_touch_type(self) -> int:
-        """获取压感类型 (2=矩阵式)"""
-        return 2
-    
-    def get_normal_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_tangential_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_approach_inc(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_touch(self) -> list:
-        return [-1] * 5
-
-    def get_thumb_matrix_touch(self,sleep_time=0):
-        return self._pressure(1)
-
-    def get_index_matrix_touch(self,sleep_time=0):
-        return self._pressure(2)
-
-    def get_middle_matrix_touch(self,sleep_time=0):
-        return self._pressure(3)
-
-    def get_ring_matrix_touch(self,sleep_time=0):
-        return self._pressure(4)
-
-    def get_little_matrix_touch(self,sleep_time=0):
-        return self._pressure(5)
-        
-    def get_matrix_touch(self) -> list:
-        """获取压感数据：矩阵式"""
-        return [self._pressure(1), self._pressure(2), self._pressure(3), 
-                self._pressure(4), self._pressure(5)]
-    
-    def get_matrix_touch_v2(self) -> list:
-        """获取压感数据：矩阵式"""
-        return self.get_matrix_touch()
-    
-    def get_torque(self) -> list:
-        """获取当前扭矩"""
-        return self.read_torques()
-    
-    def get_temperature(self) -> list:
-        """获取当前电机温度"""
-        return self.read_temperatures()
-    
-    def get_fault(self) -> list:
-        """获取当前电机故障码"""
-        return self.read_error_codes()
-    
-    def get_serial_number(self):
-        return [0] * 6
-
-    # --------------------------------------------------
-    # 便捷方法
-    # --------------------------------------------------
-    
-    def relax(self):
-        """所有手指伸直"""
-        self.set_joint_positions([255] * 6)
-    
-    def fist(self):
-        """所有手指握拳"""
-        self.set_joint_positions([0] * 6)
-    
-    def dump_status(self):
-        """打印状态信息"""
-        print("=" * 50)
-        print("L6机械手状态信息")
-        print("=" * 50)
-        
-        try:
-            # 关节状态
-            angles = self.read_angles()
-            torques = self.read_torques()
-            speeds = self.read_speeds()
-            temps = self.read_temperatures()
-            errors = self.read_error_codes()
-            
-            print("关节状态:")
-            for i, name in enumerate(self.JOINT_NAMES):
-                print(f"  {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
-                      f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
-            
-            # 版本信息
-            versions = self.read_versions()
-            print("\n版本信息:")
-            for key, value in versions.items():
-                print(f"  {key:20s}: {value}")
-            
-            # 压力传感器测试
-            print("\n压力传感器测试:")
-            thumb_pressure = self.read_pressure_thumb()
-            print(f"大拇指压力数据长度: {len(thumb_pressure)}")
-            
-        except Exception as e:
-            print(f"读取状态时出错: {e}")
-        
-        print("=" * 50)
-
-
-# ------------------- 演示程序 -------------------
-if __name__ == "__main__":
-    # 使用示例
-    try:
-        with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
-            print("连接成功!")
-            
-            # 打印状态信息
-            hand.dump_status()
-            
-            # 测试基本控制
-            print("\n测试控制功能...")
-            print("伸直手指...")
-            hand.relax()
-            time.sleep(2)
-            
-            print("握拳...")
-            hand.fist()
-            time.sleep(2)
-            
-            print("恢复伸直...")
-            hand.relax()
-            
-            # 测试压力传感器
-            print("\n测试压力传感器...")
-            thumb_matrix = hand.get_thumb_matrix_touch()
-            print(f"大拇指压力数据: {len(thumb_matrix)}个点")
-            
-            # 获取所有手指压力数据
-            all_matrices = hand.get_matrix_touch()
-            for i, name in enumerate(hand.FINGER_NAMES):
-                matrix = all_matrices[i]
-                print(f"{name}手指压力数据长度: {len(matrix)}")
-            
-    except Exception as e:
-        print(f"错误: {e}")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py

@@ -202,6 +202,11 @@ class LinkerHandApi:
         '''Get current joint state'''
         return self.hand.get_current_status()
 
+    def get_state_cached(self):
+        """Get the latest received state without transmitting new queries."""
+        getter = getattr(self.hand, "get_cached_current_status", None)
+        return getter() if getter is not None else None
+
     
     def get_state_for_pub(self):
         return self.hand.get_current_pub_status()

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py

@@ -37,11 +37,30 @@ def command_changed(previous, current):
     return any(float(old) != float(new) for old, new in zip(previous, values))
 
 
+def position_command_should_queue(previous, current, repeat=True):
+    """Whether the newest position target should be written on this heartbeat."""
+    values = list(current)
+    return bool(values) and (
+        bool(repeat) or command_changed(previous, values)
+    )
+
+
 def state_poll_due(last_poll_time, now, poll_period):
     """Keep slow CAN state reads off the latency-sensitive command path."""
     return last_poll_time is None or now >= last_poll_time + poll_period
 
 
+def state_reads_deferred(
+    last_command_time, now, quiet_period, enabled=True
+):
+    """Return whether blocking state reads must yield to active commands."""
+    return (
+        enabled
+        and last_command_time is not None
+        and now < last_command_time + quiet_period
+    )
+
+
 class LinkerHand(Node):
     def __init__(self, name):
         super().__init__(name)
@@ -54,6 +73,12 @@ class LinkerHand(Node):
         # -1 keeps the model's original startup speed. Camera teleoperation can
         # set this to a conservative value before the startup pose is sent.
         self.declare_parameter('startup_speed', -1)
+        # -1 keeps the model's original startup torque.  The retarget v2
+        # launch uses a conservative value for calibration and preview.
+        self.declare_parameter('startup_torque', -1)
+        # Preserve the legacy behaviour by default.  Safety-critical launch
+        # files can configure limits without moving to a startup pose.
+        self.declare_parameter('move_on_startup', True)
         # Empty keeps the legacy absolute topics/startup pose. A prefix lets
         # two same-side hands coexist without receiving each other's commands.
         self.declare_parameter('topic_prefix', '')
@@ -63,6 +88,17 @@ class LinkerHand(Node):
         # incoming position commands.
         self.declare_parameter('state_poll_rate', 60.0)
         self.declare_parameter('velocity_poll_rate', 60.0)
+        # G20 state and velocity reads each transmit five synchronous CAN
+        # queries. Defer them while teleoperation commands are arriving.
+        self.declare_parameter('defer_state_reads_while_commanding', True)
+        self.declare_parameter('command_quiet_period', 0.2)
+        # Legacy teleoperation sent the latest target on every 30 Hz callback,
+        # including an unchanged target.  Some firmware revisions track that
+        # cadence more smoothly than sparse change-only updates.
+        self.declare_parameter('repeat_position_commands', True)
+        # Faults stay manually clearable through cb_hand_setting_cmd. Repeated
+        # automatic clears add periodic CAN traffic to the command stream.
+        self.declare_parameter('auto_clear_faults', False)
 
         # ros时间获取
         self.stamp_clock = Clock()
@@ -75,6 +111,12 @@ class LinkerHand(Node):
         self.startup_speed = int(self.get_parameter('startup_speed').value)
         if self.startup_speed < -1 or self.startup_speed > 255:
             raise ValueError('startup_speed must be -1 or in the range [0, 255]')
+        self.startup_torque = int(self.get_parameter('startup_torque').value)
+        if self.startup_torque < -1 or self.startup_torque > 255:
+            raise ValueError('startup_torque must be -1 or in the range [0, 255]')
+        self.move_on_startup = bool(
+            self.get_parameter('move_on_startup').value
+        )
         self.topic_prefix = self.normalize_topic_prefix(
             self.get_parameter('topic_prefix').value
         )
@@ -92,6 +134,22 @@ class LinkerHand(Node):
             raise ValueError('velocity_poll_rate must be greater than zero')
         self.velocity_poll_period = 1.0 / self.velocity_poll_rate
         self.last_velocity_poll_time = None
+        self.defer_state_reads_while_commanding = bool(
+            self.get_parameter(
+                'defer_state_reads_while_commanding'
+            ).value
+        )
+        self.command_quiet_period = float(
+            self.get_parameter('command_quiet_period').value
+        )
+        if self.command_quiet_period < 0.0:
+            raise ValueError('command_quiet_period must not be negative')
+        self.auto_clear_faults = bool(
+            self.get_parameter('auto_clear_faults').value
+        )
+        self.repeat_position_commands = bool(
+            self.get_parameter('repeat_position_commands').value
+        )
         configured_startup_pose = self.get_parameter_or(
             'startup_pose',
             Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
@@ -107,6 +165,7 @@ class LinkerHand(Node):
         self.last_hand_eff_cmd = None # 最新手指力矩命令
         self.applied_hand_post_cmd = None
         self.applied_hand_vel_cmd = None
+        self.last_position_command_time = None
 
         self.last_hand_state = [-1] * 10
         self.last_hand_vel = [-1] * 10
@@ -244,26 +303,38 @@ class LinkerHand(Node):
             pose = list(self.startup_pose)
         if self.startup_speed >= 0:
             speed = [self.startup_speed] * len(speed)
+        if self.startup_torque >= 0:
+            torque = [self.startup_torque] * len(torque)
         if pose is not None:
             for i in range(1): 
                 self.api.set_speed(speed=speed)
                 time.sleep(0.1)
                 self.api.set_torque(torque=torque)
                 time.sleep(0.1)
-                self.api.finger_move(pose=pose)
-                time.sleep(0.1)
+                if self.move_on_startup:
+                    self.api.finger_move(pose=pose)
+                    time.sleep(0.1)
 
     def hand_control_cb(self, msg):
-        # The hardware can be slower than the camera.  Always replace a
-        # pending command with the newest sample and never replay an already
-        # applied sample; this prevents latency from accumulating in software.
+        # The hardware can be slower than the publisher, so a pending target is
+        # always replaced by the newest sample.  By default the newest target
+        # is also resent at the publisher cadence, matching the legacy driver.
         position = list(msg.position)
         if position:
-            self.last_hand_post_cmd = (
-                position
-                if command_changed(self.applied_hand_post_cmd, position)
-                else None
-            )
+            # Treat every valid sample as an active teleoperation heartbeat,
+            # even if integer quantisation made it identical to the previous
+            # target. This keeps all synchronous CAN diagnostics off the bus
+            # for the entire control session, matching the legacy execution
+            # path that had no state subscriber.
+            self.last_position_command_time = time.monotonic()
+            if position_command_should_queue(
+                self.applied_hand_post_cmd,
+                position,
+                self.repeat_position_commands,
+            ):
+                self.last_hand_post_cmd = position
+            else:
+                self.last_hand_post_cmd = None
 
         velocity = list(msg.velocity)
         if velocity:
@@ -286,6 +357,9 @@ class LinkerHand(Node):
             self.api.finger_move(pose=pose)
             self.applied_hand_post_cmd = pose
             self.last_hand_post_cmd = None
+            cached_state = self.api.get_state_cached()
+            if cached_state is not None:
+                self.last_hand_state = cached_state
 
         if self.last_hand_vel_cmd is not None:
             vel = list(self.last_hand_vel_cmd)
@@ -320,6 +394,15 @@ class LinkerHand(Node):
         if self.hand_state_pub.get_subscription_count() < 1:
             return
         now = time.monotonic()
+        if state_reads_deferred(
+            self.last_position_command_time,
+            now,
+            self.command_quiet_period,
+            self.defer_state_reads_while_commanding,
+        ):
+            # pub_state continues to publish the last completed state as a
+            # heartbeat. A fresh blocking read is made after motion settles.
+            return
         if not state_poll_due(
             self.last_state_poll_time, now, self.state_poll_period
         ):
@@ -342,6 +425,12 @@ class LinkerHand(Node):
         # Position commands have priority over synchronous state reads.
         self._apply_pending_commands()
         self._poll_state_if_due()
+        diagnostics_deferred = state_reads_deferred(
+            self.last_position_command_time,
+            time.monotonic(),
+            self.command_quiet_period,
+            self.defer_state_reads_while_commanding,
+        )
         if self.cmd_lock == False:
             time.sleep(0.003)
             if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
@@ -360,7 +449,11 @@ class LinkerHand(Node):
                 if self.run_count == 7:
                     self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
                 time.sleep(0.005)
-            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+            if (
+                self.run_count == 8
+                and self.hand_info_pub.get_subscription_count() > 0
+                and not diagnostics_deferred
+            ):
                 """手部信息"""
                 self.last_hand_info = {
                     "version": self.embedded_version, # Dexterous hand version number
@@ -375,8 +468,9 @@ class LinkerHand(Node):
                     "finger_order": self.api.get_finger_order() # Finger motor order
                 }
                 
-            if self.run_count == 9:
+            if self.run_count == 9 and self.auto_clear_faults:
                 self.api.clear_faults() # 自动清除错误编码
+            if self.run_count == 9:
                 self.run_count = 0
             self.run_count += 1
             time.sleep(0.003)

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak

@@ -1,414 +0,0 @@
-#!/usr/bin/env python3 
-# -*- coding: utf-8 -*-
-'''
-编译: colcon build --symlink-install
-启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
-'''
-from re import A
-import rclpy,sys                                     # ROS2 Python接口库
-import time
-import numpy as np
-from rclpy.node import Node                      # ROS2 节点类
-from rclpy.clock import Clock
-from std_msgs.msg import String, Header, Float32MultiArray
-from sensor_msgs.msg import JointState, PointCloud2, PointField
-import time, json, threading
-from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
-from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
-from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
-
-
-class LinkerHand(Node):
-    def __init__(self, name):
-        super().__init__(name)
-        # 声明参数（带默认值）
-        self.declare_parameter('hand_type', 'left')
-        self.declare_parameter('hand_joint', 'L6')
-        self.declare_parameter('is_touch', False)
-        self.declare_parameter('can', 'can0')
-        self.declare_parameter('modbus', "None")        
-
-        # ros时间获取
-        self.stamp_clock = Clock()
-        # 获取参数值
-        self.hand_type = self.get_parameter('hand_type').value
-        self.hand_joint = self.get_parameter('hand_joint').value
-        self.is_touch = self.get_parameter('is_touch').value
-        self.can = self.get_parameter('can').value
-        self.modbus = self.get_parameter('modbus').value
-        self.sdk_v = 2
-        self.sleep_time = 0.005
-        self.cmd_lock = False
-        self.last_hand_post_cmd = None # 最新手指位置命令
-        self.last_hand_vel_cmd = None # 最新手指速度命令
-        self.last_hand_eff_cmd = None # 最新手指力矩命令
-
-        self.last_hand_state = [-1] * 10
-        self.last_hand_vel = [-1] * 10
-        self.force = [[-1] * 5] * 4
-        self.matrix_dic = {
-            "stamp":{
-                "sec": 0,
-                "nanosec": 0,
-            },
-            "thumb_matrix":[[-1] * 6 for _ in range(12)],
-            "index_matrix":[[-1] * 6 for _ in range(12)],
-            "middle_matrix":[[-1] * 6 for _ in range(12)],
-            "ring_matrix":[[-1] * 6 for _ in range(12)],
-            "little_matrix":[[-1] * 6 for _ in range(12)]
-        }
-        # 压感矩阵合值，单位g 克
-        self.matrix_mass_dic = {
-            "stamp":{
-                "secs": 0,
-                "nsecs": 0,
-            },
-            "thumb_mass":[-1],
-            "index_mass":[-1],
-            "middle_mass":[-1],
-            "ring_mass":[-1],
-            "little_mass":[-1]
-        }
-        self.last_hand_info = {
-            "version": [-1], # Dexterous hand version number
-            "hand_joint": self.hand_joint, # Dexterous hand joint type
-            "speed": [-1] * 10, # Current speed threshold of the dexterous hand
-            "current": [-1] * 10, # Current of the dexterous hand
-            "fault": [-1] * 10, # Current fault of the dexterous hand
-            "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
-            "torque": [-1] * 10, # Current torque of the dexterous hand
-            "is_touch":self.is_touch,
-            "touch_type": -1,
-            "finger_order": None # Finger motor order
-        }
-        self.version = []
-        self.touch_type = -1
-        self.hz = 1.0/60.0
-
-        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
-        self._init_hand()
-        time.sleep(1)
-        self.run_count = 0 # 计数器，用于记录运行次数
-        self.timer = self.create_timer(0.01, self.run)  # 100 Hz
-        self.thread_pub_state = threading.Thread(target=self.pub_state)
-        self.thread_pub_state.daemon = True
-        self.thread_pub_state.start()
-
-    def _init_hand(self):
-        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
-        time.sleep(0.1)
-        self.touch_type = self.api.get_touch_type()
-        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
-        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
-        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
-        if self.is_touch == True:
-            if self.touch_type > 1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
-                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
-                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
-                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
-            elif self.touch_type != -1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
-                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
-            else:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
-                self.is_touch = False
-        self.embedded_version = self.api.get_embedded_version()
-        pose = None
-        torque = [200, 200, 200, 200, 200]
-        speed = [200, 250, 250, 250, 250]
-        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
-            pose = [200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250]
-            # O6 最大速度阈值
-            speed = [200, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L7":
-            # The data length of L7 is 7, reinitialize here
-            pose = [255, 200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250, 250]
-            speed = [120, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L10":
-            torque = [255] * 10
-            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
-            speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L20":
-            pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
-        elif self.hand_joint == "L21":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        elif self.hand_joint == "L25":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        if pose is not None:
-            for i in range(1): 
-                self.api.set_speed(speed=speed)
-                time.sleep(0.1)
-                self.api.set_torque(torque=torque)
-                time.sleep(0.1)
-                self.api.finger_move(pose=pose)
-                time.sleep(0.1)
-
-    def list_check(self,pose):
-        if isinstance(pose, list) == False:
-            return False
-        if len(self.last_hand_post_cmd) != len(pose):
-            return False
-        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
-
-    def hand_control_cb(self, msg):
-        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
-            self.last_hand_post_cmd = msg.position
-        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
-            self.last_hand_vel_cmd = msg.velocity
-        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
-            self.last_hand_eff_cmd = msg.effort
-
-    def run(self):
-        if self.sdk_v == 1:
-            self.sleep_time = 0.009
-        if self.hand_state_pub.get_subscription_count() > 0:
-            # 优先获取手指状态并且发布
-            self.last_hand_state = self.api.get_state()
-            time.sleep(0.003)
-            self.last_hand_vel = self.api.get_joint_speed()
-            time.sleep(0.002)
-        if self.cmd_lock == False:
-            if self.last_hand_post_cmd != None:
-                self.api.finger_move(pose=self.last_hand_post_cmd)
-                self.last_hand_post_cmd = None
-            if self.last_hand_vel_cmd != None:
-                vel = list(self.last_hand_vel_cmd)
-                if all(x == 0 for x in vel):
-                    pass
-                else:
-                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L7" and len(vel) == 7:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L10" and len(vel) == 10:
-                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L20" and len(vel) == 20:
-                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L21" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L25" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                self.last_hand_vel_cmd = None
-            time.sleep(0.003)
-            if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                """单点式压力传感器"""
-                self.force = self.api.get_force()
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                """矩阵式压力传感器"""
-                if self.run_count == 3:
-                    self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 4:
-                    self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 5:
-                    self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 6:
-                    self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 7:
-                    self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
-                time.sleep(0.005)
-            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
-                """手部信息"""
-                self.last_hand_info = {
-                    "version": self.embedded_version, # Dexterous hand version number
-                    "hand_joint": self.hand_joint, # Dexterous hand joint type
-                    "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
-                    "current": self.api.get_current(), # Current of the dexterous hand
-                    "fault": self.api.get_fault(), # Current fault of the dexterous hand
-                    "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
-                    "torque": self.api.get_torque(), # Current torque of the dexterous hand
-                    "is_touch":self.is_touch,
-                    "touch_type": self.touch_type,
-                    "finger_order": self.api.get_finger_order() # Finger motor order
-                }
-            if self.run_count == 9:
-                self.run_count = 0
-            self.run_count += 1
-            time.sleep(0.003)
-
-
-    def pub_state(self):
-        while True:
-            if self.hand_state_pub.get_subscription_count() > 0:
-                msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
-                self.hand_state_pub.publish(msg)
-            if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                msg = Float32MultiArray()
-                msg.data = [float(val) for sublist in self.force for val in sublist]
-                self.touch_pub.publish(msg)
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                # 发布矩阵压感数据JSON格式
-                self.pub_matrix_dic()
-                # 发布矩阵压感和值JSON格式
-                self.pub_matrix_mass(dic=self.matrix_dic)
-                # 发布矩阵压感点云格式
-                self.pub_matrix_point_cloud()
-            if self.hand_info_pub.get_subscription_count() > 0:
-                msg = String()
-                msg.data = json.dumps(self.last_hand_info)
-                self.hand_info_pub.publish(msg)
-            time.sleep(self.hz)
-
-    def pub_matrix_mass(self, dic):
-        """发布矩阵数据合值 单位g 克 JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_mass_dic["stamp"]["secs"] = t_secs
-        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
-        self.matrix_mass_dic["unit"] = "g"
-        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
-        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
-        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
-        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
-        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
-        msg.data = json.dumps(self.matrix_mass_dic)
-        self.matrix_touch_mass_pub.publish(msg)
-
-    def pub_matrix_point_cloud(self):
-        """发布矩阵数据点云格式"""
-        tmp_dic = self.matrix_dic.copy()
-        del tmp_dic['stamp']               # 去掉时间戳字段
-        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数
-        # 摊平到一维：360 个 float
-        flat_list = [v for frame in all_matrices for v in frame]  # 360
-        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
-        fields = [PointField(
-            name='val',
-            offset=0,
-            datatype=PointField.UINT8,
-            count=1
-        )]
-        pc = PointCloud2()
-        pc.header.stamp = self.stamp_clock.now().to_msg()
-        pc.header.frame_id = ''
-        pc.height = 1
-        pc.width = flat.size         # 360
-        pc.fields = fields
-        pc.is_bigendian = False
-        pc.point_step = 1            # 1 个 float32
-        pc.row_step = pc.point_step * pc.width
-        pc.data = flat.tobytes()     # 1440 字节
-        self.matrix_touch_pub_pc.publish(pc)
-
-    def pub_matrix_dic(self):
-        """发布矩阵数据JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_dic["stamp"]["secs"] = t_secs
-        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
-        msg.data = json.dumps(self.matrix_dic)
-        self.matrix_touch_pub.publish(msg)
-
-    def joint_state_msg(self, pose,vel=[]):
-        joint_state = JointState()
-        joint_state.header = Header()
-        joint_state.header.stamp = self.get_clock().now().to_msg()
-        joint_state.name = self.api.get_finger_order()
-        joint_state.position = [float(x) for x in pose]
-        if len(vel) > 1:
-            joint_state.velocity = [float(x) for x in vel]
-        else:
-            joint_state.velocity = [0.0] * len(pose)
-        joint_state.effort = [0.0] * len(pose)
-        return joint_state
-    
-    
-
-    
-
-    def hand_setting_cb(self,msg):
-        '''控制命令回调'''
-        data = json.loads(msg.data)
-        print(f"Received setting command: {data['setting_cmd']}",flush=True)
-        try:
-            if data["params"]["hand_type"] == "left":
-                hand = self.api
-                hand_left = True
-            elif data["params"]["hand_type"] == "right":
-                hand = self.api
-                hand_right = True
-            else:
-                print("Please specify the hand part to be set",flush=True)
-                return
-            self.cmd_lock = True
-            # Set maximum torque
-            if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
-                torque = list(data["params"]["torque"])
-                hand.set_torque(torque=torque)
-                
-            if data["setting_cmd"] == "set_speed": # Set speed
-                if isinstance(data["params"]["speed"], list) == True:
-                    speed = data["params"]["speed"]
-                    hand.set_speed(speed=speed)
-                else:
-                    ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
-            if data["setting_cmd"] == "clear_faults": # Clear faults
-                if hand_left == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 left hand cannot clear faults")
-                elif hand_right == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 right hand cannot clear faults")
-                else:
-                    hand.clear_faults()
-            if data["setting_cmd"] == "get_faults": # Get faults
-                f = hand.get_fault()
-                ColorMsg(msg=f"Get faults: {f}")
-            if data["setting_cmd"] == "electric_current": # Get current
-                ColorMsg(msg=f"Get current: {hand.get_current()}")
-            if data["setting_cmd"] == "set_electric_current": # Set current
-                if isinstance(data["params"]["current"], list) == True:
-                    hand.set_current(data["params"]["current"])
-            if data["setting_cmd"] == "show_fun_table": # Get faults
-                f = hand.show_fun_table()
-        except:
-            print("命令参数错误")
-            self.cmd_lock = False
-        finally:
-            self.cmd_lock = False
-
-
-    def close_can(self):
-        self.api.open_can.close_can(can=self.can)
-        sys.exit(0)
-
-        
-def main(args=None):
-    try:
-        rclpy.init(args=args)
-        node = LinkerHand("linker_hand_sdk")
-        embedded_version = node.embedded_version
-        if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) == 6 and node.hand_joint == "L10":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        else:
-            ColorMsg(msg=f"SDK V1", color="green")
-            node.sdk_v = 1
-        rclpy.spin(node)         # 主循环，监听 ROS 回调
-    except KeyboardInterrupt:
-        print("收到 Ctrl+C，准备退出...")
-    finally:
-        # node.close_can()         # 关闭 CAN 或其他硬件资源
-        # node.destroy_node()      # 销毁 ROS 节点
-        # rclpy.shutdown()         # 关闭 ROS
-        print("程序已退出。")

src/linker_hand_ros2_sdk/test/test_topic_prefix.py

@@ -4,7 +4,9 @@ from linker_hand_ros2_sdk.linker_hand import (
     COMMAND_QOS,
     LinkerHand,
     command_changed,
+    position_command_should_queue,
     state_poll_due,
+    state_reads_deferred,
 )
 
 
@@ -30,7 +32,25 @@ def test_identical_commands_are_not_reapplied():
     assert not command_changed([60, 60], [])
 
 
+def test_legacy_heartbeat_resends_unchanged_position_target():
+    assert position_command_should_queue([60, 60], [60, 60], repeat=True)
+    assert not position_command_should_queue(
+        [60, 60], [60, 60], repeat=False
+    )
+    assert position_command_should_queue(
+        [60, 60], [60, 61], repeat=False
+    )
+    assert not position_command_should_queue([60, 60], [], repeat=True)
+
+
 def test_state_polling_is_throttled_without_missing_deadline():
     assert state_poll_due(None, 10.0, 0.1)
     assert not state_poll_due(10.0, 10.09, 0.1)
     assert state_poll_due(10.0, 10.1, 0.1)
+
+
+def test_state_reads_yield_to_recent_motion_then_resume():
+    assert state_reads_deferred(10.0, 10.1, 0.2)
+    assert not state_reads_deferred(10.0, 10.2, 0.2)
+    assert not state_reads_deferred(None, 10.1, 0.2)
+    assert not state_reads_deferred(10.0, 10.1, 0.2, enabled=False)

src/linkerhand_retarget/launch/ffg_dual_g20_o6.launch.py

@@ -0,0 +1,230 @@
+"""One left FFG, G20 on can0 and O6 on can1, fail-closed by default."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+from linkerhand_retarget.motion.linkerforce_v2.constants import (
+    DEFAULT_COMMAND_FILTER_CONFIG,
+    DEFAULT_INPUT_FILTER_ENABLED,
+)
+
+
+G20_OPEN_POSE = [
+    255, 255, 255, 255, 255,
+    255, 193, 148, 105, 42,
+    255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255,
+]
+O6_OPEN_POSE = [255, 179, 255, 255, 255, 255]
+
+
+def generate_launch_description():
+    package_share = FindPackageShare("linkerhand_retarget")
+    g20_seed = PathJoinSubstitution(
+        [package_share, "linkerforce_v2", "profiles", "g20_seed_profile.json"]
+    )
+    o6_seed = PathJoinSubstitution(
+        [package_share, "linkerforce_v2", "profiles", "o6_seed_profile.json"]
+    )
+    arguments = [
+        DeclareLaunchArgument("input_mode", default_value="serial"),
+        DeclareLaunchArgument(
+            "raw_input_topic", default_value="/ffg/left/raw_joint_state"
+        ),
+        DeclareLaunchArgument("serial_port", default_value=""),
+        DeclareLaunchArgument("baudrate", default_value="0"),
+        DeclareLaunchArgument("auto_scan", default_value="true"),
+        DeclareLaunchArgument("publish_rate", default_value="30.0"),
+        DeclareLaunchArgument(
+            "input_filter_enabled",
+            default_value=str(DEFAULT_INPUT_FILTER_ENABLED).lower(),
+        ),
+        DeclareLaunchArgument(
+            "input_filter_process_variance", default_value="0.00001"
+        ),
+        DeclareLaunchArgument(
+            "input_filter_measurement_variance", default_value="0.0005"
+        ),
+        DeclareLaunchArgument(
+            "input_filter_reset_gap", default_value="0.35"
+        ),
+        DeclareLaunchArgument(
+            "command_filter_mode",
+            default_value=str(DEFAULT_COMMAND_FILTER_CONFIG["mode"]),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_ema_alpha",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["ema_alpha"]
+            ),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_max_step_u8",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["max_step_u8"]
+            ),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_deadband_u8",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["deadband_u8"]
+            ),
+        ),
+        DeclareLaunchArgument("glove_profile", default_value=""),
+        DeclareLaunchArgument("wear_check", default_value=""),
+        DeclareLaunchArgument("g20_profile", default_value=g20_seed),
+        DeclareLaunchArgument("o6_profile", default_value=o6_seed),
+        DeclareLaunchArgument("g20_serial_number", default_value=""),
+        DeclareLaunchArgument("o6_serial_number", default_value=""),
+        DeclareLaunchArgument("g20_can", default_value="can0"),
+        DeclareLaunchArgument("o6_can", default_value="can1"),
+        DeclareLaunchArgument("startup_speed", default_value="30"),
+        DeclareLaunchArgument("startup_torque", default_value="80"),
+        DeclareLaunchArgument("move_on_startup", default_value="false"),
+        DeclareLaunchArgument("g20_state_poll_rate", default_value="10.0"),
+        DeclareLaunchArgument("o6_state_poll_rate", default_value="30.0"),
+        DeclareLaunchArgument(
+            "defer_state_reads_while_commanding", default_value="true"
+        ),
+        DeclareLaunchArgument("command_quiet_period", default_value="0.2"),
+        DeclareLaunchArgument("auto_clear_faults", default_value="false"),
+    ]
+
+    g20_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_g20_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "is_touch": False,
+                "can": LaunchConfiguration("g20_can"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "startup_pose": G20_OPEN_POSE,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("startup_speed"), value_type=int
+                ),
+                "startup_torque": ParameterValue(
+                    LaunchConfiguration("startup_torque"), value_type=int
+                ),
+                "move_on_startup": ParameterValue(
+                    LaunchConfiguration("move_on_startup"), value_type=bool
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_state_poll_rate"), value_type=float
+                ),
+                "velocity_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_state_poll_rate"), value_type=float
+                ),
+                "defer_state_reads_while_commanding": ParameterValue(
+                    LaunchConfiguration("defer_state_reads_while_commanding"),
+                    value_type=bool,
+                ),
+                "command_quiet_period": ParameterValue(
+                    LaunchConfiguration("command_quiet_period"),
+                    value_type=float,
+                ),
+                "auto_clear_faults": ParameterValue(
+                    LaunchConfiguration("auto_clear_faults"), value_type=bool
+                ),
+            }
+        ],
+    )
+    o6_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_o6_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "O6",
+                "is_touch": False,
+                "can": LaunchConfiguration("o6_can"),
+                "modbus": "None",
+                "topic_prefix": "/o6",
+                "startup_pose": O6_OPEN_POSE,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("startup_speed"), value_type=int
+                ),
+                "startup_torque": ParameterValue(
+                    LaunchConfiguration("startup_torque"), value_type=int
+                ),
+                "move_on_startup": ParameterValue(
+                    LaunchConfiguration("move_on_startup"), value_type=bool
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("o6_state_poll_rate"), value_type=float
+                ),
+            }
+        ],
+    )
+    retarget = Node(
+        package="linkerhand_retarget",
+        executable="ffg_dual_retarget",
+        name="ffg_dual_retarget",
+        output="screen",
+        parameters=[
+            {
+                "input_mode": LaunchConfiguration("input_mode"),
+                "raw_input_topic": LaunchConfiguration("raw_input_topic"),
+                "serial_port": LaunchConfiguration("serial_port"),
+                "baudrate": ParameterValue(
+                    LaunchConfiguration("baudrate"), value_type=int
+                ),
+                "auto_scan": ParameterValue(
+                    LaunchConfiguration("auto_scan"), value_type=bool
+                ),
+                "publish_rate": ParameterValue(
+                    LaunchConfiguration("publish_rate"), value_type=float
+                ),
+                "input_filter_enabled": ParameterValue(
+                    LaunchConfiguration("input_filter_enabled"),
+                    value_type=bool,
+                ),
+                "input_filter_process_variance": ParameterValue(
+                    LaunchConfiguration("input_filter_process_variance"),
+                    value_type=float,
+                ),
+                "input_filter_measurement_variance": ParameterValue(
+                    LaunchConfiguration("input_filter_measurement_variance"),
+                    value_type=float,
+                ),
+                "input_filter_reset_gap": ParameterValue(
+                    LaunchConfiguration("input_filter_reset_gap"),
+                    value_type=float,
+                ),
+                "command_filter_mode": LaunchConfiguration(
+                    "command_filter_mode"
+                ),
+                "command_filter_ema_alpha": ParameterValue(
+                    LaunchConfiguration("command_filter_ema_alpha"),
+                    value_type=float,
+                ),
+                "command_filter_max_step_u8": ParameterValue(
+                    LaunchConfiguration("command_filter_max_step_u8"),
+                    value_type=float,
+                ),
+                "command_filter_deadband_u8": ParameterValue(
+                    LaunchConfiguration("command_filter_deadband_u8"),
+                    value_type=float,
+                ),
+                "glove_profile": LaunchConfiguration("glove_profile"),
+                "wear_check": LaunchConfiguration("wear_check"),
+                "g20_profile": LaunchConfiguration("g20_profile"),
+                "o6_profile": LaunchConfiguration("o6_profile"),
+                "g20_serial_number": LaunchConfiguration("g20_serial_number"),
+                "o6_serial_number": LaunchConfiguration("o6_serial_number"),
+                "g20_can_interface": LaunchConfiguration("g20_can"),
+                "o6_can_interface": LaunchConfiguration("o6_can"),
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [g20_driver, o6_driver, retarget])

src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml

@@ -27,8 +27,8 @@ serial:
     baudrate: 2000000
     port: /dev/ttyUSB0
   right:
-    baudrate: 2000000
-    port: /dev/ttyUSB0
+    baudrate: null
+    port: null
   serial_debug: false
 system:
   can:
@@ -40,9 +40,9 @@ system:
   motion_device: eric
   motion_type: linkerforce
   retargeting_type: projection
-  rightpub: true
+  rightpub: false
   robotname_l: g20
-  robotname_r: l10
+  robotname_r: g20
   sapientype: left
   usecan: false
   usegui: false
@@ -51,6 +51,6 @@ system:
   useudp: true
   useudpserver: true
 udp:
-  ip: 0.0.0.0
+  ip: 192.168.11.88
   port: 8888
   serverport: 5551

src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml

@@ -16,7 +16,7 @@ commandlower_left_l20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, Non
 commandupper_left_l20: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0] 
 commandlower_right_g20: [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, None, None, None, None, 255, 255, 255, 255, 255]
 commandupper_right_g20: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, None, None, None, None, 0, 0, 0, 0, 0]
-commandlower_left_g20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandlower_left_g20: [255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
 commandupper_left_g20: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0] 
 commandlower_right_l7: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
 commandupper_right_l7: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]

src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkerforce.py

@@ -210,6 +210,8 @@ class FrameHandler:
         self.logger = logger
         self._data_lock = threading.Lock()
         self._poslist: List[float] = [0.0] * 21
+        self._position_sequence = 0
+        self._last_position_monotonic = 0.0
         self._forcelist: List[float] = [0.0] * 5
         self._realforcelist: List[int] = [0] * 5
 
@@ -221,7 +223,20 @@ class FrameHandler:
     @poslist.setter
     def poslist(self, value: List[float]):
         with self._data_lock:
-            self._poslist = value
+            self._poslist = value.copy()
+            self._position_sequence += 1
+            self._last_position_monotonic = time.monotonic()
+
+    @property
+    def position_snapshot(self) -> tuple[List[float], int, float]:
+        """Return values, sequence and monotonic receive time atomically."""
+
+        with self._data_lock:
+            return (
+                self._poslist.copy(),
+                self._position_sequence,
+                self._last_position_monotonic,
+            )
 
     @property
     def forcelist(self) -> List[float]:
@@ -514,6 +529,10 @@ class ForceSerialReader:
     def poslist(self, value: List[float]):
         self._handler.poslist = value
 
+    @property
+    def position_snapshot(self) -> tuple[List[float], int, float]:
+        return self._handler.position_snapshot
+
     @property
     def forcelist(self) -> List[float]:
         return self._handler.forcelist
@@ -729,4 +748,4 @@ class ForceSerialReader:
             'checked_ports': list(self.checked_ports),
             'exclude_ports': list(self.exclude_ports),
             'baudrates': self.baudrates
-        }
+        }

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config copy.py

@@ -0,0 +1,473 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+    # 含义解释：
+    # robot_idx：URDF关节序列
+
+    # 拇指旋转3个关节的加权系数，人手的0/1/2序列，对应URDF的第1关节(下标0)
+    'thumb_rotate': {
+        'name': '拇指旋转',
+        'joints': [1, 2],
+        'weights': {
+            'v1': [1, 0],
+            'v2': [1, 0]
+        },
+        'robot_idx': 0,
+        'type': 'thumb',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.0,
+            'extended_exp_factor': 1.0
+        }
+    },  
+    # 拇指侧摆3个关节的加权系数，人手的0/1/2序列，对应URDF的第2关节(下标1)
+    'thumb_abduction': {
+        'name': '拇指侧摆',
+        'joints': [0, 1, 2],
+        'weights': {
+            'v1': [0, 1, 0],
+            'v2': [0, 1, 0]
+        },
+        'robot_idx': 1,
+        'type': 'thumb',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 1.0
+        }
+    },
+    # 拇指根部弯曲3个关节的加权系数，人手的2/3/4序列，对应URDF的第3关节(下标2)
+    'thumb_root_flexion': {
+        'name': '拇指根部弯曲',
+        'joints': [2, 3, 4],
+        'weights': {
+            'v1': [1, 0, 0],
+            'v2': [1, 0, 0]
+        },
+        'robot_idx': 2,
+        'type': 'thumb',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None, 
+        # {  
+        #     'trigger_finger': 'thumb_abduction',
+        #     'threshold': 0.3,
+        #     'low_weight_config': {
+        #         'joints': [2, 3, 4],
+        #         'weights': [1, 0, 0],
+        #         'reverse_motion': False
+        #     },
+        #     'high_weight_config': {
+        #         'joints': [2, 3, 4],
+        #         'weights': [0.3, 0.0, 0.7],
+        #         'reverse_motion': False
+        #     }
+        # },
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 10
+        }
+    },
+    # 拇指指尖弯曲3个关节的加权系数，人手的2/3/4序列，对应URDF的第4关节(下标3)
+    'thumb_end_flexion': {
+        'name': '拇指指尖弯曲',
+        'joints': [2, 3, 4],
+        'weights': {
+            'v1': [0, 0.0, 1],
+            'v2': [0, 0.0, 1]
+        },
+        'robot_idx': 3,
+        'type': 'thumb',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1,
+            'extended_exp_factor': 20
+        }
+    },
+    # 食指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+    'index_roll': {
+        'name': '食指',
+        'joints': [5],
+        'weights': {
+            'v1': [1],
+            'v2': [1]
+        },
+        'robot_idx': 5,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': False,
+            'scale_factor': 1.0,
+        }
+    },
+    # 食指弯曲（根部弯曲）的加权系数，人身的6/7/8序列，对应URDF的第4关节(下标3)
+    'index_root_flexion': {
+        'name': '食指',
+        'joints': [6, 7, 8],
+        'weights': {
+            'v1': [1, 0.0, 0],
+            'v2': [1, 0.0, 0]
+        },
+        'robot_idx': 6,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 5
+        }
+    },
+    # 食指弯曲（末端弯曲）的加权系数，人手的6/7/8序列，对应URDF的第4关节(下标3)
+    'index_end_flexion': {
+        'name': '食指',
+        'joints': [6, 7, 8],
+        'weights': {
+            'v1': [0, 0.0, 1],
+            'v2': [0, 0.0, 1]
+        },
+        'robot_idx': 7,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1,
+            'extended_exp_factor': 30
+        }
+    },
+    # 中指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+    'middle_roll': {
+        'name': '中指',
+        'joints': [9],
+        'weights': {
+            'v1': [1],
+            'v2': [1]
+        },
+        'robot_idx': 9,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.0,
+            'extended_exp_factor': 1.0
+        }
+    },
+    # 中指弯曲（根部弯曲）的加权系数，人手的10/11/12序列，对应URDF的第6关节(下标5)
+    'middle_root_flexion': {
+        'name': '中指',
+        'joints': [10, 11, 12],
+        'weights': {
+            'v1': [1, 0.0, 0],
+            'v2': [1, 0.0, 0]
+        },
+        'robot_idx': 10,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 5
+        }
+    },
+    # 中指弯曲（末端弯曲）的加权系数，人手的10/11/12序列，对应URDF的第6关节(下标5)
+    'middle_end_flexion': {
+        'name': '中指',
+        'joints': [10, 11, 12],
+        'weights': {
+            'v1': [0, 0.0, 1],
+            'v2': [0, 0.0, 1]
+        },
+        'robot_idx': 11,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1,
+            'extended_exp_factor': 30
+        }
+    },
+    # 无名指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+    'ring_roll': {
+        'name': '无名指',
+        'joints': [13],
+        'weights': {
+            'v1': [1],
+            'v2': [1]
+        },
+        'robot_idx': 13,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': False,
+            'scale_factor': 1.0,
+        }
+    },
+    # 无名指弯曲（根部弯曲）的加权系数，人手的14/15/16序列，对应URDF的第8关节(下标7)
+    'ring_root_flexion': {
+        'name': '无名指',
+        'joints': [14, 15, 16],
+        'weights': {
+            'v1': [1, 0.0, 0],
+            'v2': [1, 0.0, 0]
+        },
+        'robot_idx': 14,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 5
+        }
+    },
+    # 无名指弯曲（末端弯曲）的加权系数，人手的14/15/16序列，对应URDF的第8关节(下标7)
+    'ring_end_flexion': {
+        'name': '无名指',
+        'joints': [14, 15, 16],
+        'weights': {
+            'v1': [0, 0.0, 1],
+            'v2': [0, 0.0, 1]
+        },
+        'robot_idx': 15,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1,
+            'extended_exp_factor': 30
+        }
+    },
+    # 小指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+    'pinky_roll': {
+        'name': '小指',
+        'joints': [17],
+        'weights': {
+            'v1': [1],
+            'v2': [1]
+        },
+        'robot_idx': 17,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': False,
+            'scale_factor': 1.0,
+        }
+    },
+    # 小指弯曲（根部弯曲）的加权系数，人手的18/19/20序列，对应URDF的第10关节(下标9)
+    'pinky_root_flexion': {
+        'name': '小指',
+        'joints': [18, 19, 20],
+        'weights': {
+            'v1': [1, 0.0, 0],
+            'v2': [1, 0.0, 0]
+        },
+        'robot_idx': 18,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1.2,
+            'extended_exp_factor': 5
+        }
+    },
+    # 小指弯曲（末端弯曲）的加权系数，人手的18/19/20序列，对应URDF的第10关节(下标9)
+    'pinky_end_flexion': {
+        'name': '小指',
+        'joints': [18, 19, 20],
+        'weights': {
+            'v1': [0, 0.0, 1],
+            'v2': [0, 0.0, 1]
+        },
+        'robot_idx': 19,
+        'type': 'finger',
+        'reverse_motion': {
+            'v1': False,
+            'v2': False
+        },
+        'dynamic_weight': None,
+        'extended_mapping': {
+            'enabled': True,
+            'scale_factor': 1,
+            'extended_exp_factor': 30
+        }
+    }
+}
+
+# 映射顺序
+MAPPING_ORDER = [
+    'thumb_rotate', 'thumb_abduction', 'thumb_root_flexion', 'thumb_end_flexion', 
+    'index_roll', 'index_root_flexion', 'index_end_flexion', 
+    'middle_roll', 'middle_root_flexion', 'middle_end_flexion', 
+    'ring_roll', 'ring_root_flexion','ring_end_flexion', 
+    'pinky_roll', 'pinky_root_flexion', 'pinky_end_flexion'
+]
+
+# 三态默认配置
+MULTI_SEGMENT_CONFIG = {
+    'states': [
+        'original', 
+        'opose', 
+        # 'fist'  # 取消注释启用三段映射
+    ],
+    'state_names': {
+        'original': '张手',
+        'opose': 'O手势',
+        # 'fist': '握拳'
+    }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_ORIGINAL_LEFT = [
+    0.0, 0.0, 0.0, 0.0, 0.0, 
+    0.2, 0.0, 0.0, 0.0, 
+    0.0, 0.0, 0.0, 0.0, 
+    -0.2, 0.0, 0.0, 0.0, 
+    -0.2, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+    0.0, 0.0, 0.0, 0.0, 0.0, 
+    -0.2, 0.0, 0.0, 0.0, 
+    0.0, 0.0, 0.0, 0.0, 
+    0.2, 0.0, 0.0, 0.0, 
+    0.2, 0.0, 0.0, 0.0
+]
+
+ROBOT_OPOSE_LEFT = [
+    0.6, 1.2, 0.5, 0.6, 0.0, 
+    0.0, 0.7, 1.08, 0.00, 
+    0.0, 0.7, 1.08, 0.00 , 
+    0.0, 0.7, 1.08, 0.00, 
+    0.0, 0.7, 1.08, 0.00 
+]
+
+ROBOT_OPOSE_RIGHT = [
+    0.6, 1.2, 0.5, 0.6, 0.0, 
+    0.0, 0.7, 1.08, 0.00, 
+    0.0, 0.7, 1.08, 0.00, 
+    0.0, 0.7, 1.08, 0.00, 
+    0.0, 0.7, 1.08, 0.00
+]
+
+ROBOT_FIST_RIGHT = [
+    1.39, 1.57, 0.83, 1.25, 1.29,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55
+]
+
+ROBOT_FIST_LEFT = [
+    1.39, 1.57, 0.83, 1.25, 1.29,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55,
+    0, 1.22, 1.75, 1.55
+]
+
+# 电机输出约束配置 (20电机)
+MOTOR_CONSTRAINTS = {
+    'left': [
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 80, 'max': 255, 'enabled': True},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+    ],
+    'right': [
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 80, 'max': 255, 'enabled': True},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+        {'min': 0, 'max': 255, 'enabled': False},
+    ]
+}

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py

@@ -432,9 +432,7 @@ MOTOR_CONSTRAINTS = {
         {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
-        # Left G20 thumb abduction is enabled by hand_config.yml. Keep the
-        # physically verified command away from the mechanical end stop.
-        {'min': 20, 'max': 255, 'enabled': True},
+        {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
@@ -472,4 +470,4 @@ MOTOR_CONSTRAINTS = {
         {'min': 0, 'max': 255, 'enabled': False},
         {'min': 0, 'max': 255, 'enabled': False},
     ]
-}
+}

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l10_config.py

@@ -126,7 +126,7 @@ FINGER_CONFIGS = {
         'type': 'finger',
         'reverse_motion': {
             'v1': False,
-            'v2': True
+            'v2': False
         },
         'dynamic_weight': None,
         'extended_mapping': {

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py

@@ -10,13 +10,6 @@ from ..config.g20_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT
 from typing import List
 from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
 
-
-# The left G20 ring abduction motor runs opposite to the glove's corresponding
-# roll sensor. Keep this override local to LeftHand so the pinky and right-hand
-# mappings retain their native directions.
-LEFT_REVERSED_SIDE_SWINGS = ('ring_roll',)
-
-
 def _resolve_version_config(configs: dict, version: str) -> dict:
     """
     解析版本配置，将字典格式的 weights/reverse_motion 转换为具体值
@@ -307,7 +300,6 @@ class LeftHand:
 
         finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
         self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
-        self._apply_side_direction_overrides()
 
         for config_name, config in FINGER_CONFIGS.items():
             if config.get('dynamic_weight'):
@@ -315,10 +307,6 @@ class LeftHand:
 
         self.motor_constraints = MOTOR_CONSTRAINTS['left']
 
-    def _apply_side_direction_overrides(self):
-        for finger_name in LEFT_REVERSED_SIDE_SWINGS:
-            self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = True
-
     def _apply_motor_constraints(self):
         for i, constraint in enumerate(self.motor_constraints):
             if constraint.get('enabled', False):
@@ -347,8 +335,6 @@ class LeftHand:
                 if version_key in config['reverse_motion']:
                     self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
 
-        self._apply_side_direction_overrides()
-
     def initialize_mapper(self) -> bool:
         """
         初始化映射器
@@ -532,3 +518,4 @@ class LeftHand:
             self.g_jointvelocity[i] = 255
             self.last_jointvelocity[i] = self.g_jointvelocity[i]
             self.last_jointpositions[i] = self.g_jointpositions[i]
+

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py

@@ -1247,7 +1247,7 @@ class Retarget():
                 self.node.get_logger().error("标定失败，退出程序")
                 return
             self.calibration = -1
-        self.node.create_timer(1.0/100, self.process_callback)  # 100Hz
+        self.node.create_timer(1.0/30, self.process_callback)  # 30Hz
 
     def stop_serial_threads(self):
         """停止串口线程，在 destroy_node 时调用"""

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/__init__.py

@@ -0,0 +1,18 @@
+"""Profile-driven FFG retargeting for heterogeneous Linker Hands."""
+
+from .mapping import HandIntentExtractor, RobotMapper
+from .profiles import (
+    ProfileError,
+    load_glove_profile,
+    load_robot_profile,
+    profile_sha256,
+)
+
+__all__ = [
+    "HandIntentExtractor",
+    "ProfileError",
+    "RobotMapper",
+    "load_glove_profile",
+    "load_robot_profile",
+    "profile_sha256",
+]

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibrate_glove.py

@@ -0,0 +1,244 @@
+"""Interactive multi-gesture FFG calibration CLI."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+from pathlib import Path
+import time
+from typing import Sequence
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .calibration import build_glove_profile, robust_summary
+from .constants import (
+    FFG_JOINT_NAMES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+)
+from .profiles import load_glove_profile, save_profile
+
+
+GESTURE_LABELS = {
+    "open_spread": "五指自然张开、自然分开",
+    "fingers_together": "五指伸直并拢",
+    "tabletop": "桌面手势（根部弯曲、末端伸直）",
+    "hook": "钩拳（根部伸直、末端弯曲）",
+    "fist": "自然握拳",
+    "thumb_max_out": "拇指最大外展",
+    "thumb_across": "拇指横跨掌心",
+    "pinch_index": "拇指—食指捏合",
+    "pinch_middle": "拇指—中指捏合",
+    "pinch_ring": "拇指—无名指捏合",
+    "pinch_pinky": "拇指—小指捏合",
+    "index_flex_sweep": "食指独立弯曲往返",
+    "middle_flex_sweep": "中指独立弯曲往返",
+    "ring_flex_sweep": "无名指独立弯曲往返",
+    "pinky_flex_sweep": "小指独立弯曲往返",
+    "thumb_flex_sweep": "拇指弯曲往返",
+    "thumb_opposition_sweep": "拇指对掌往返",
+    "finger_splay_sweep": "四指开合往返",
+}
+
+
+class CaptureNode(Node):
+    def __init__(self, topic: str) -> None:
+        super().__init__("ffg_calibration_capture")
+        self.capture_active = False
+        self.frames: list[list[float]] = []
+        self.invalid_frames = 0
+        self.subscription = self.create_subscription(
+            JointState, topic, self._callback, 50
+        )
+
+    def _callback(self, message: JointState) -> None:
+        if not self.capture_active:
+            return
+        if list(message.name) != list(FFG_JOINT_NAMES):
+            self.invalid_frames += 1
+            return
+        values = [float(value) for value in message.position]
+        if len(values) != 21 or not all(np.isfinite(values)):
+            self.invalid_frames += 1
+            return
+        self.frames.append(values)
+
+    def collect(self, duration: float) -> list[list[float]]:
+        self.frames = []
+        self.invalid_frames = 0
+        self.capture_active = True
+        deadline = time.monotonic() + duration
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.02)
+        self.capture_active = False
+        return [frame.copy() for frame in self.frames]
+
+
+def _capture_repeats(
+    node: CaptureNode,
+    gesture: str,
+    *,
+    duration: float,
+    repeats: int,
+    minimum: int,
+) -> tuple[list[list[float]], list[dict[str, object]]]:
+    all_frames: list[list[float]] = []
+    summaries: list[dict[str, object]] = []
+    for repeat in range(1, repeats + 1):
+        input(
+            f"\n[{gesture}] {GESTURE_LABELS[gesture]}，"
+            f"第{repeat}/{repeats}次。摆好后按Enter开始采集："
+        )
+        frames = node.collect(duration)
+        if len(frames) < minimum:
+            raise RuntimeError(
+                f"{gesture} 第{repeat}次只有{len(frames)}个有效帧，"
+                f"要求至少{minimum}；无效命名/数据帧={node.invalid_frames}"
+            )
+        summary = robust_summary(frames, 21)
+        summary["repeat"] = repeat
+        summaries.append(summary)
+        all_frames.extend(frames)
+        print(f"  已保存 {len(frames)} 帧")
+    return all_frames, summaries
+
+
+def _quick_check(node: CaptureNode, profile_path: str, args: argparse.Namespace) -> int:
+    profile = load_glove_profile(profile_path)
+    failures: list[str] = []
+    errors: dict[str, float] = {}
+    for gesture in ("open_spread", "fist", "pinch_index"):
+        frames, _ = _capture_repeats(
+            node,
+            gesture,
+            duration=args.duration,
+            repeats=1,
+            minimum=args.min_samples,
+        )
+        observed = np.median(np.asarray(frames, dtype=float), axis=0)
+        reference = np.asarray(profile["poses"][gesture]["median"], dtype=float)
+        mad = np.asarray(profile["poses"][gesture]["mad"], dtype=float)
+        normalized_error = float(
+            np.sqrt(np.mean(np.square((observed - reference) / np.maximum(6 * mad, 0.05))))
+        )
+        print(f"  {gesture}: normalized_error={normalized_error:.3f}")
+        errors[gesture] = normalized_error
+        if normalized_error > args.quick_threshold:
+            failures.append(gesture)
+    attestation = {
+        "schema_version": 1,
+        "kind": "ffg_wear_check",
+        "checked_at": datetime.now(timezone.utc).isoformat(),
+        "glove_profile_sha256": profile["_profile_sha256"],
+        "threshold": args.quick_threshold,
+        "normalized_errors": errors,
+        "passed": not failures,
+    }
+    output = args.quick_output or str(
+        Path(profile_path).expanduser().resolve().with_suffix(".wear_check.json")
+    )
+    save_profile(attestation, output)
+    if failures:
+        print(
+            f"快速佩戴检查失败: {', '.join(failures)}；"
+            f"请重新佩戴或完整标定。结果已保存: {output}"
+        )
+        return 2
+    print(f"快速佩戴检查通过，凭据已保存: {output}")
+    return 0
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="FFG左手套多手势标定")
+    parser.add_argument("--topic", default="/ffg/left/raw_joint_state")
+    parser.add_argument("--output")
+    parser.add_argument("--glove-id")
+    parser.add_argument("--operator")
+    parser.add_argument("--firmware", default="unknown")
+    parser.add_argument("--duration", type=float, default=2.0)
+    parser.add_argument("--repeats", type=int, default=3)
+    parser.add_argument("--min-samples", type=int, default=50)
+    parser.add_argument("--dynamic-duration", type=float, default=3.0)
+    parser.add_argument("--skip-dynamic", action="store_true")
+    parser.add_argument("--quick-check", metavar="PROFILE")
+    parser.add_argument("--quick-output")
+    parser.add_argument("--quick-threshold", type=float, default=4.0)
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    if not args.quick_check and not (args.output and args.glove_id and args.operator):
+        raise SystemExit(
+            "完整标定必须提供 --output、--glove-id 和 --operator"
+        )
+    if args.duration <= 0 or args.repeats < 1 or args.min_samples < 1:
+        raise SystemExit("duration/repeats/min-samples参数无效")
+    rclpy.init(args=ros_args)
+    node = CaptureNode(args.topic)
+    try:
+        if args.quick_check:
+            return _quick_check(node, args.quick_check, args)
+
+        static_captures: dict[str, list[list[float]]] = {}
+        repeat_summaries: dict[str, list[dict[str, object]]] = {}
+        print(
+            "开始FFG完整标定。每个静态姿势保持"
+            f"{args.duration:.1f}s、重复{args.repeats}次；"
+            "采集期间保持姿势稳定。"
+        )
+        for gesture in GLOVE_STATIC_GESTURES:
+            frames, summaries = _capture_repeats(
+                node,
+                gesture,
+                duration=args.duration,
+                repeats=args.repeats,
+                minimum=args.min_samples,
+            )
+            static_captures[gesture] = frames
+            repeat_summaries[gesture] = summaries
+
+        dynamic_captures: dict[str, list[list[float]]] = {}
+        dynamic_repeat_summaries: dict[str, list[dict[str, object]]] = {}
+        if not args.skip_dynamic:
+            print("\n开始短时往返轨迹，每种动作重复3次。")
+            for gesture in GLOVE_DYNAMIC_GESTURES:
+                frames, summaries = _capture_repeats(
+                    node,
+                    gesture,
+                    duration=args.dynamic_duration,
+                    repeats=3,
+                    minimum=args.min_samples,
+                )
+                dynamic_captures[gesture] = frames
+                dynamic_repeat_summaries[gesture] = summaries
+
+        profile = build_glove_profile(
+            static_captures,
+            glove_id=args.glove_id,
+            operator=args.operator,
+            firmware=args.firmware,
+            dynamic_captures=dynamic_captures,
+            repeat_summaries=repeat_summaries,
+            dynamic_repeat_summaries=dynamic_repeat_summaries,
+        )
+        output = save_profile(profile, args.output)
+        validated = load_glove_profile(output)
+        print(
+            f"\n标定完成: {output}\n"
+            f"approved_for_runtime={validated['approved_for_runtime']}\n"
+            f"sha256={validated['_profile_sha256']}\n"
+            "启动实机前请再执行 --quick-check。"
+        )
+        return 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibrate_robot.py

@@ -0,0 +1,519 @@
+"""Interactive G20/O6 pose-profile capture from namespaced ROS topics."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import json
+from pathlib import Path
+import threading
+import time
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .calibration import build_robot_profile
+from .constants import MODEL_COMMAND_LENGTHS
+from .profiles import load_robot_profile, save_profile
+
+
+POSE_LABELS = {
+    "open_spread": "张手",
+    "fingers_together": "五指并拢",
+    "tabletop": "桌面手势",
+    "hook": "钩拳",
+    "fist": "握拳",
+    "thumb_max_out": "拇指最大外展",
+    "thumb_across": "拇指横跨掌心",
+    "pinch_index": "拇指—食指捏合",
+    "pinch_middle": "拇指—中指捏合",
+    "pinch_ring": "拇指—无名指捏合",
+    "pinch_pinky": "拇指—小指捏合",
+}
+
+
+class HandPoseCaptureNode(Node):
+    def __init__(
+        self,
+        model: str,
+        command_topic: str,
+        state_topic: str,
+        snapshot_topic: str,
+        command_names: Sequence[str],
+    ) -> None:
+        super().__init__(f"{model.lower()}_pose_capture")
+        self.model = model
+        self.length = MODEL_COMMAND_LENGTHS[model]
+        self.command_names = [str(name) for name in command_names]
+        self.latest_command: list[float] | None = None
+        self.latest_state: list[float] | None = None
+        self.latest_state_names: list[str] | None = None
+        self.latest_snapshot: list[float] | None = None
+        self.snapshot_sequence = 0
+        self.command_subscription = self.create_subscription(
+            JointState, command_topic, self._command_callback, 20
+        )
+        self.state_subscription = self.create_subscription(
+            JointState, state_topic, self._state_callback, 20
+        )
+        self.snapshot_subscription = self.create_subscription(
+            JointState, snapshot_topic, self._snapshot_callback, 20
+        )
+
+    def _valid(self, values: Sequence[float]) -> bool:
+        return (
+            len(values) == self.length
+            and all(np.isfinite(values))
+            and all(0.0 <= float(value) <= 255.0 for value in values)
+        )
+
+    def _command_callback(self, message: JointState) -> None:
+        if not self._valid(message.position):
+            return
+        incoming_names = [str(name) for name in message.name]
+        if (
+            len(incoming_names) != self.length
+            or set(incoming_names) != set(self.command_names)
+        ):
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(incoming_names, message.position)
+        }
+        self.latest_command = [lookup[name] for name in self.command_names]
+
+    def _snapshot_callback(self, message: JointState) -> None:
+        if not self._valid(message.position):
+            return
+        names = [str(name) for name in message.name]
+        if len(names) != self.length or set(names) != set(self.command_names):
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(names, message.position)
+        }
+        self.latest_snapshot = [lookup[name] for name in self.command_names]
+        self.snapshot_sequence += 1
+
+    def _state_callback(self, message: JointState) -> None:
+        names = [str(name) for name in message.name]
+        if (
+            self._valid(message.position)
+            and len(names) == self.length
+            and len(set(names)) == self.length
+            and set(names) == set(self.command_names)
+        ):
+            lookup = {
+                name: float(value)
+                for name, value in zip(names, message.position)
+            }
+            self.latest_state = [lookup[name] for name in self.command_names]
+            self.latest_state_names = list(self.command_names)
+
+
+def _default_status(model: str, pose: str) -> str:
+    if model == "O6" and pose in {
+        "tabletop",
+        "hook",
+        "pinch_middle",
+        "pinch_ring",
+        "pinch_pinky",
+    }:
+        return "approximate"
+    return "exact"
+
+
+def _checkpoint_path(output: str, configured: str | None) -> Path:
+    if configured:
+        return Path(configured).expanduser().resolve()
+    output_path = Path(output).expanduser().resolve()
+    return output_path.with_suffix(".checkpoint.json")
+
+
+def _checkpoint_document(
+    *,
+    model: str,
+    seed: Mapping[str, Any],
+    output: str,
+    serial_number: str,
+    firmware: str,
+    can_interface: str,
+    operator: str,
+    captures: Mapping[str, Mapping[str, Any]],
+    completed: bool = False,
+    final_profile_sha256: str | None = None,
+) -> dict[str, Any]:
+    return {
+        "schema_version": 1,
+        "kind": "robot_pose_capture_checkpoint",
+        "updated_at": datetime.now(timezone.utc).isoformat(),
+        "model": model,
+        "side": "left",
+        "seed_profile_sha256": seed["_profile_sha256"],
+        "output": str(Path(output).expanduser().resolve()),
+        "device": {
+            "serial_number": serial_number,
+            "firmware": firmware,
+            "can_interface": can_interface,
+        },
+        "operator": operator,
+        "command_names": list(seed["command_names"]),
+        "required_poses": list(seed["required_poses"]),
+        "captures": dict(captures),
+        "completed": bool(completed),
+        "final_profile_sha256": final_profile_sha256,
+    }
+
+
+def _command_bound_violations(
+    profile: Mapping[str, Any],
+    command: Sequence[float],
+) -> tuple[str, ...]:
+    try:
+        values = np.asarray(command, dtype=float)
+        bounds = np.asarray(profile["command_bounds"], dtype=float)
+    except (KeyError, TypeError, ValueError):
+        return ("命令或安全范围格式无效",)
+    names = list(profile.get("command_names", ()))
+    if values.shape != (len(names),) or bounds.shape != (len(names), 2):
+        return ("命令长度或安全范围长度无效",)
+    violations = []
+    for index, (value, (minimum, maximum)) in enumerate(zip(values, bounds)):
+        if (
+            not np.isfinite(value)
+            or value < minimum
+            or value > maximum
+        ):
+            violations.append(
+                f"{names[index]}={value:g}，允许[{minimum:g}, {maximum:g}]"
+            )
+    return tuple(violations)
+
+
+def _load_checkpoint(
+    path: Path,
+    *,
+    model: str,
+    seed: Mapping[str, Any],
+    output: str,
+    serial_number: str,
+    firmware: str,
+    can_interface: str,
+    operator: str,
+) -> dict[str, dict[str, Any]]:
+    try:
+        checkpoint = json.loads(path.read_text(encoding="utf-8"))
+    except (OSError, json.JSONDecodeError) as exc:
+        raise RuntimeError(f"无法读取姿势检查点 {path}: {exc}") from exc
+    expected = {
+        "kind": "robot_pose_capture_checkpoint",
+        "model": model,
+        "side": "left",
+        "seed_profile_sha256": seed["_profile_sha256"],
+        "output": str(Path(output).expanduser().resolve()),
+        "operator": operator,
+        "command_names": list(seed["command_names"]),
+        "required_poses": list(seed["required_poses"]),
+    }
+    for field, value in expected.items():
+        if checkpoint.get(field) != value:
+            raise RuntimeError(
+                f"检查点字段{field}与本次标定不匹配；"
+                "确认命令参数，或使用--fresh明确重新开始"
+            )
+    expected_device = {
+        "serial_number": serial_number,
+        "firmware": firmware,
+        "can_interface": can_interface,
+    }
+    if checkpoint.get("device") != expected_device:
+        raise RuntimeError(
+            "检查点的SN/固件/CAN与本次标定不匹配；"
+            "确认命令参数，或使用--fresh明确重新开始"
+        )
+    captures = checkpoint.get("captures")
+    if not isinstance(captures, dict):
+        raise RuntimeError("姿势检查点中的captures无效")
+    required = set(seed["required_poses"])
+    names = list(seed["command_names"])
+    length = MODEL_COMMAND_LENGTHS[model]
+    validated: dict[str, dict[str, Any]] = {}
+    for pose, capture in captures.items():
+        if pose not in required or not isinstance(capture, dict):
+            raise RuntimeError(f"姿势检查点包含无效动作: {pose}")
+        command = capture.get("cmd_u8")
+        state = capture.get("state_u8")
+        try:
+            command_values = np.asarray(command, dtype=float)
+            state_values = np.asarray(state, dtype=float)
+        except (TypeError, ValueError):
+            command_values = np.asarray([], dtype=float)
+            state_values = np.asarray([], dtype=float)
+        if (
+            not isinstance(command, list)
+            or not isinstance(state, list)
+            or command_values.shape != (length,)
+            or state_values.shape != (length,)
+            or not np.all(np.isfinite(command_values))
+            or not np.all(np.isfinite(state_values))
+            or np.any(command_values < 0.0)
+            or np.any(command_values > 255.0)
+            or np.any(state_values < 0.0)
+            or np.any(state_values > 255.0)
+            or capture.get("command_names") != names
+            or capture.get("state_names") != names
+            or capture.get("status")
+            not in {"exact", "approximate", "unsupported"}
+            or capture.get("confirmed") is not True
+        ):
+            raise RuntimeError(f"姿势检查点中的{pose}数据无效")
+        validated[pose] = dict(capture)
+    return validated
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="G20/O6实机姿势profile捕获")
+    parser.add_argument("--model", required=True, choices=("G20", "O6", "g20", "o6"))
+    parser.add_argument("--seed", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--serial-number", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--firmware", default="unknown")
+    parser.add_argument("--can-interface")
+    parser.add_argument("--command-topic")
+    parser.add_argument("--state-topic")
+    parser.add_argument("--snapshot-topic")
+    parser.add_argument(
+        "--checkpoint",
+        help="检查点文件；默认使用输出文件名加.checkpoint.json",
+    )
+    parser.add_argument(
+        "--fresh",
+        action="store_true",
+        help="忽略并覆盖已有检查点，从第一个姿势重新开始",
+    )
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    model = args.model.upper()
+    lower = model.lower()
+    can_interface = args.can_interface or ("can0" if model == "G20" else "can1")
+    command_topic = args.command_topic or f"/{lower}/cb_left_hand_control_cmd"
+    state_topic = args.state_topic or f"/{lower}/cb_left_hand_state"
+    snapshot_topic = (
+        args.snapshot_topic or f"/{lower}/calibration_pose_snapshot"
+    )
+    seed = load_robot_profile(args.seed, model)
+    required_poses = list(seed.get("required_poses", ()))
+    if not required_poses:
+        raise SystemExit("seed profile不包含required_poses，不能用于姿势采集")
+    checkpoint_path = _checkpoint_path(args.output, args.checkpoint)
+    captures: dict[str, dict[str, Any]] = {}
+    if checkpoint_path.is_file() and not args.fresh:
+        captures = _load_checkpoint(
+            checkpoint_path,
+            model=model,
+            seed=seed,
+            output=args.output,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+        )
+        unsafe_poses = {
+            pose: _command_bound_violations(seed, capture["cmd_u8"])
+            for pose, capture in captures.items()
+            if _command_bound_violations(seed, capture["cmd_u8"])
+        }
+        if unsafe_poses:
+            print("检查点中发现超出profile安全范围的姿势，将只重拍这些姿势：")
+            for pose, violations in unsafe_poses.items():
+                print(
+                    f"  {POSE_LABELS.get(pose, pose)}: "
+                    + "; ".join(violations)
+                )
+                captures.pop(pose)
+    save_profile(
+        _checkpoint_document(
+            model=model,
+            seed=seed,
+            output=args.output,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+            captures=captures,
+        ),
+        checkpoint_path,
+    )
+
+    rclpy.init(args=ros_args)
+    node = HandPoseCaptureNode(
+        model,
+        command_topic,
+        state_topic,
+        snapshot_topic,
+        seed["command_names"],
+    )
+    executor = SingleThreadedExecutor()
+    executor.add_node(node)
+    spin_thread = threading.Thread(target=executor.spin, daemon=True)
+    spin_thread.start()
+    try:
+        print(
+            f"{model}姿势标定开始。请使用低速、低力矩的命名空间GUI独立调节；"
+            "不要用旧手套映射，也不要带电强掰。\n"
+            f"监听命令: {command_topic}\n监听状态: {state_topic}\n"
+            f"检查点: {checkpoint_path}"
+        )
+        if captures:
+            print(
+                "已从检查点恢复 "
+                f"{len(captures)}/{len(required_poses)} 个姿势: "
+                + ", ".join(captures)
+            )
+        for pose in required_poses:
+            if pose in captures:
+                print(
+                    f"\n跳过已保存姿势：{POSE_LABELS.get(pose, pose)}"
+                )
+                continue
+            while pose not in captures:
+                default = _default_status(model, pose)
+                snapshot_sequence = node.snapshot_sequence
+                print(
+                    f"\n将{model}调到“{POSE_LABELS.get(pose, pose)}”。"
+                    "确认通道、方向和姿势正确且实机稳定后，"
+                    "点击GUI的“保存当前标定姿势”；终端正在自动等待快照……",
+                    flush=True,
+                )
+                while (
+                    rclpy.ok()
+                    and node.snapshot_sequence <= snapshot_sequence
+                ):
+                    time.sleep(0.05)
+                if not rclpy.ok():
+                    raise RuntimeError("ROS已停止，无法继续姿势标定")
+                command = node.latest_snapshot
+                state = node.latest_state
+                state_names = node.latest_state_names
+                if command is None or node.latest_command is None:
+                    print(
+                        f"未从{command_topic}收到有效{model}命令；"
+                        "请检查GUI命名空间后重新保存当前姿势。"
+                    )
+                    continue
+                if state is None or state_names is None:
+                    print(
+                        f"未从{state_topic}收到有效{model}状态；"
+                        "请检查驱动后重新保存当前姿势。"
+                    )
+                    continue
+                violations = _command_bound_violations(seed, command)
+                if violations:
+                    print(
+                        "当前GUI快照超出profile安全范围，不会保存："
+                        + "; ".join(violations)
+                        + "。请在安全范围内重新调整并保存。"
+                    )
+                    continue
+                redo = False
+                while True:
+                    entered = input(
+                        "手势状态 exact/approximate/unsupported "
+                        f"[{default}]（输入r重拍）："
+                    ).strip().lower()
+                    if entered == "r":
+                        redo = True
+                        break
+                    status = entered or default
+                    if status in {"exact", "approximate", "unsupported"}:
+                        break
+                    print(f"无效手势状态: {status}，请重新输入。")
+                if redo:
+                    print("当前快照已放弃，请调整后重新点击GUI保存。")
+                    continue
+                captures[pose] = {
+                    "cmd_u8": command,
+                    "command_names": list(seed["command_names"]),
+                    "state_u8": state,
+                    "state_names": state_names,
+                    "status": status,
+                    "confirmed": True,
+                    "confirmation_method": "gui_snapshot_and_status",
+                    "captured_at": datetime.now(timezone.utc).isoformat(),
+                }
+                save_profile(
+                    _checkpoint_document(
+                        model=model,
+                        seed=seed,
+                        output=args.output,
+                        serial_number=args.serial_number,
+                        firmware=args.firmware,
+                        can_interface=can_interface,
+                        operator=args.operator,
+                        captures=captures,
+                    ),
+                    checkpoint_path,
+                )
+                print(
+                    f"已写入检查点：{len(captures)}/{len(required_poses)}"
+                )
+
+        approve = (
+            input(
+                "\n是否批准该profile用于低速实机控制？只有全部姿势已确认才会生效。[y/N]："
+            )
+            .strip()
+            .lower()
+            == "y"
+        )
+        profile = build_robot_profile(
+            seed,
+            captures,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+            approved_for_control=approve,
+        )
+        output = save_profile(profile, args.output)
+        validated = load_robot_profile(output, model)
+        save_profile(
+            _checkpoint_document(
+                model=model,
+                seed=seed,
+                output=args.output,
+                serial_number=args.serial_number,
+                firmware=args.firmware,
+                can_interface=can_interface,
+                operator=args.operator,
+                captures=captures,
+                completed=True,
+                final_profile_sha256=validated["_profile_sha256"],
+            ),
+            checkpoint_path,
+        )
+        print(
+            f"\nprofile已保存: {output}\n"
+            f"approved_for_control={validated['approved_for_control']}\n"
+            f"sha256={validated['_profile_sha256']}\n"
+            f"检查点已标记完成并保留: {checkpoint_path}"
+        )
+        return 0
+    finally:
+        executor.shutdown()
+        spin_thread.join(timeout=1.0)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibration.py

@@ -0,0 +1,475 @@
+"""Profile generation from robust multi-gesture calibration captures."""
+
+from __future__ import annotations
+
+import copy
+from datetime import datetime, timezone
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .constants import (
+    BASE_INTENT_NAMES,
+    FFG_JOINT_NAMES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+    MODEL_COMMAND_LENGTHS,
+    PINCH_ROLES,
+    SCHEMA_VERSION,
+    gesture_intent_value,
+)
+
+
+def robust_summary(samples: Sequence[Sequence[float]], length: int) -> dict[str, Any]:
+    """Return raw frames and robust statistics for a calibration capture."""
+
+    values = np.asarray(samples, dtype=float)
+    if values.ndim != 2 or values.shape[1] != length:
+        raise ValueError(f"capture must have shape (N, {length})")
+    if values.shape[0] < 1 or not np.all(np.isfinite(values)):
+        raise ValueError("capture contains no valid finite samples")
+    median = np.median(values, axis=0)
+    mad = np.median(np.abs(values - median), axis=0)
+    return {
+        "sample_count": int(values.shape[0]),
+        "median": median.astype(float).tolist(),
+        "mad": mad.astype(float).tolist(),
+        "raw_frames": values.astype(float).tolist(),
+    }
+
+
+_FEATURE_POSE_LABELS: dict[str, tuple[list[int], dict[str, float]]] = {
+    "thumb_rotate": (
+        [1, 2],
+        {"open_spread": 0.5, "thumb_max_out": 0.0, "thumb_across": 1.0},
+    ),
+    "thumb_abduction": (
+        [0, 1, 2],
+        {"open_spread": 0.5, "thumb_max_out": 0.0, "thumb_across": 1.0},
+    ),
+    "thumb_opposition": (
+        [0, 1, 2, 3, 4],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "thumb_across": 1.0,
+            "pinch_index": 1.0,
+            "pinch_middle": 1.0,
+            "pinch_ring": 1.0,
+            "pinch_pinky": 1.0,
+        },
+    ),
+    "thumb_root": (
+        [2, 3],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "fist": 1.0,
+            "thumb_across": 0.75,
+        },
+    ),
+    "thumb_tip": (
+        [3, 4],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "fist": 1.0,
+            "thumb_across": 0.75,
+        },
+    ),
+}
+
+for _finger, _base in (("index", 5), ("middle", 9), ("ring", 13), ("pinky", 17)):
+    _FEATURE_POSE_LABELS[f"{_finger}_root"] = (
+        [_base + 1, _base + 2, _base + 3],
+        {
+            "open_spread": 0.0,
+            "fingers_together": 0.0,
+            "hook": 0.0,
+            "tabletop": 1.0,
+            "fist": 1.0,
+        },
+    )
+    _FEATURE_POSE_LABELS[f"{_finger}_tip"] = (
+        [_base + 1, _base + 2, _base + 3],
+        {
+            "open_spread": 0.0,
+            "fingers_together": 0.0,
+            "tabletop": 0.0,
+            "hook": 1.0,
+            "fist": 1.0,
+        },
+    )
+    _FEATURE_POSE_LABELS[f"{_finger}_splay"] = (
+        [_base],
+        {"fingers_together": 0.0, "open_spread": 1.0},
+    )
+
+
+def _fit_feature(
+    pose_medians: Mapping[str, np.ndarray],
+    indices: Sequence[int],
+    labels: Mapping[str, float],
+) -> dict[str, Any]:
+    available = [(pose, float(label)) for pose, label in labels.items() if pose in pose_medians]
+    zeros = [pose_medians[pose][indices] for pose, label in available if label <= 0.05]
+    ones = [pose_medians[pose][indices] for pose, label in available if label >= 0.95]
+    if not zeros or not ones:
+        raise ValueError("feature calibration requires both zero and one endpoint poses")
+    low = np.median(np.vstack(zeros), axis=0)
+    high = np.median(np.vstack(ones), axis=0)
+    span = high - low
+    x_rows: list[np.ndarray] = []
+    y: list[float] = []
+    for pose, label in available:
+        row = np.zeros(len(indices), dtype=float)
+        valid = np.abs(span) > 1e-6
+        row[valid] = (pose_medians[pose][indices][valid] - low[valid]) / span[valid]
+        x_rows.append(np.clip(row, 0.0, 1.0))
+        y.append(label)
+    weights, *_ = np.linalg.lstsq(np.vstack(x_rows), np.asarray(y), rcond=None)
+    weights = np.clip(weights, 0.0, None)
+    if float(np.sum(weights)) <= 1e-9:
+        weights = np.ones(len(indices), dtype=float)
+    weights /= np.sum(weights)
+    return {
+        "indices": [int(index) for index in indices],
+        "weights": weights.astype(float).tolist(),
+        "low": low.astype(float).tolist(),
+        "high": high.astype(float).tolist(),
+    }
+
+
+def build_glove_profile(
+    captures: Mapping[str, Sequence[Sequence[float]]],
+    *,
+    glove_id: str,
+    operator: str,
+    firmware: str = "",
+    dynamic_captures: Mapping[str, Sequence[Sequence[float]]] | None = None,
+    repeat_summaries: Mapping[str, Sequence[Mapping[str, Any]]] | None = None,
+    dynamic_repeat_summaries:
+        Mapping[str, Sequence[Mapping[str, Any]]] | None = None,
+) -> dict[str, Any]:
+    """Build a complete left FFG glove profile from eleven pose captures."""
+
+    missing = set(GLOVE_STATIC_GESTURES) - set(captures)
+    if missing:
+        raise ValueError(f"missing glove calibration gestures: {sorted(missing)}")
+    summaries = {
+        gesture: robust_summary(captures[gesture], 21)
+        for gesture in GLOVE_STATIC_GESTURES
+    }
+    for gesture, repeats in (repeat_summaries or {}).items():
+        if gesture in summaries:
+            summaries[gesture]["repeats"] = copy.deepcopy(list(repeats))
+    pose_medians = {
+        gesture: np.asarray(summary["median"], dtype=float)
+        for gesture, summary in summaries.items()
+    }
+    features = {
+        name: _fit_feature(pose_medians, *_FEATURE_POSE_LABELS[name])
+        for name in BASE_INTENT_NAMES
+    }
+
+    open_pose = pose_medians["open_spread"]
+    pinch_anchors: dict[str, Any] = {}
+    finger_indices = {
+        "index": list(range(5, 9)),
+        "middle": list(range(9, 13)),
+        "ring": list(range(13, 17)),
+        "pinky": list(range(17, 21)),
+    }
+    for role in PINCH_ROLES:
+        indices = list(range(5)) + finger_indices[role]
+        center = pose_medians[f"pinch_{role}"][indices]
+        open_values = open_pose[indices]
+        pose_mad = np.asarray(summaries[f"pinch_{role}"]["mad"], dtype=float)[indices]
+        scale = np.maximum.reduce(
+            [np.abs(center - open_values), pose_mad * 6.0, np.full(len(indices), 0.02)]
+        )
+        open_distance = float(
+            np.sqrt(np.mean(np.square((open_values - center) / scale)))
+        )
+        pinch_anchors[role] = {
+            "indices": indices,
+            "center": center.astype(float).tolist(),
+            "scale": scale.astype(float).tolist(),
+            "open_distance": max(open_distance, 1e-3),
+        }
+
+    dynamic = {
+        name: robust_summary(samples, 21)
+        for name, samples in (dynamic_captures or {}).items()
+    }
+    for gesture, repeats in (dynamic_repeat_summaries or {}).items():
+        if gesture in dynamic:
+            dynamic[gesture]["repeats"] = copy.deepcopy(list(repeats))
+
+    def complete_repeats(summary: Mapping[str, Any]) -> bool:
+        repeats = summary.get("repeats")
+        return (
+            isinstance(repeats, list)
+            and len(repeats) == 3
+            and all(
+                isinstance(repeat, Mapping)
+                and int(repeat.get("sample_count", 0)) >= 50
+                for repeat in repeats
+            )
+            and sum(int(repeat["sample_count"]) for repeat in repeats)
+            == int(summary.get("sample_count", 0))
+        )
+
+    approved_for_runtime = (
+        all(complete_repeats(summary) for summary in summaries.values())
+        and set(GLOVE_DYNAMIC_GESTURES).issubset(dynamic)
+        and all(
+            complete_repeats(dynamic[name])
+            for name in GLOVE_DYNAMIC_GESTURES
+        )
+    )
+    return {
+        "schema_version": SCHEMA_VERSION,
+        "profile_kind": "ffg_glove",
+        "calibration": "provisional",
+        "approved_for_runtime": approved_for_runtime,
+        "created_at": datetime.now(timezone.utc).isoformat(),
+        "glove": {
+            "id": str(glove_id),
+            "side": "left",
+            "operator": str(operator),
+            "firmware": str(firmware),
+        },
+        "joint_names": list(FFG_JOINT_NAMES),
+        "poses": summaries,
+        "dynamic_captures": dynamic,
+        "features": features,
+        "pinch_anchors": pinch_anchors,
+    }
+
+
+def _fit_source_weights(
+    model_profile: Mapping[str, Any],
+    channel: Mapping[str, Any],
+    pose_commands: Mapping[str, Sequence[float]],
+) -> dict[str, float]:
+    sources = list(
+        model_profile.get("fit_sources", {}).get(str(channel["name"]), [])
+    )
+    if not sources:
+        return {
+            str(name): float(weight)
+            for name, weight in channel["sources"].items()
+        }
+    command_index = int(channel["index"])
+    open_command = float(pose_commands["open_spread"][command_index])
+    fist_command = float(pose_commands["fist"][command_index])
+    span = fist_command - open_command
+    if abs(span) < 1e-6:
+        return {source: 1.0 / len(sources) for source in sources}
+    gestures = [
+        pose for pose in ("open_spread", "tabletop", "hook", "fist") if pose in pose_commands
+    ]
+    x = np.asarray(
+        [
+            [gesture_intent_value(gesture, source) for source in sources]
+            for gesture in gestures
+        ],
+        dtype=float,
+    )
+    y = np.asarray(
+        [
+            (float(pose_commands[gesture][command_index]) - open_command) / span
+            for gesture in gestures
+        ],
+        dtype=float,
+    )
+    weights, *_ = np.linalg.lstsq(x, y, rcond=None)
+    weights = np.clip(weights, 0.0, None)
+    if float(np.sum(weights)) <= 1e-9:
+        weights = np.ones(len(sources), dtype=float)
+    weights /= np.sum(weights)
+    return {source: float(weight) for source, weight in zip(sources, weights)}
+
+
+def _aggregate_points(points: Sequence[Sequence[float]]) -> list[list[float]]:
+    buckets: dict[float, list[float]] = {}
+    for raw_x, raw_y in points:
+        buckets.setdefault(float(raw_x), []).append(float(raw_y))
+    return [
+        [float(x), float(np.median(values))]
+        for x, values in sorted(buckets.items())
+    ]
+
+
+def _monotonic_points(points: Sequence[Sequence[float]]) -> list[list[float]]:
+    """Aggregate duplicate activations and fit monotonic command endpoints."""
+
+    ordered = [
+        (point[0], point[1])
+        for point in _aggregate_points(points)
+    ]
+    if len(ordered) < 2:
+        raise ValueError("calibration produced fewer than two activation points")
+    direction = 1.0 if ordered[-1][1] >= ordered[0][1] else -1.0
+    blocks: list[dict[str, Any]] = []
+    for index, (_, value) in enumerate(ordered):
+        blocks.append(
+            {"start": index, "end": index, "sum": direction * value, "count": 1}
+        )
+        while len(blocks) >= 2:
+            left = blocks[-2]
+            right = blocks[-1]
+            left_mean = float(left["sum"]) / int(left["count"])
+            right_mean = float(right["sum"]) / int(right["count"])
+            if left_mean <= right_mean:
+                break
+            blocks[-2:] = [
+                {
+                    "start": left["start"],
+                    "end": right["end"],
+                    "sum": float(left["sum"]) + float(right["sum"]),
+                    "count": int(left["count"]) + int(right["count"]),
+                }
+            ]
+    fitted = np.zeros(len(ordered), dtype=float)
+    for block in blocks:
+        mean = direction * float(block["sum"]) / int(block["count"])
+        fitted[int(block["start"]) : int(block["end"]) + 1] = mean
+    return [
+        [float(point[0]), float(value)]
+        for point, value in zip(ordered, fitted)
+    ]
+
+
+def build_robot_profile(
+    seed_profile: Mapping[str, Any],
+    captures: Mapping[str, Mapping[str, Any]],
+    *,
+    serial_number: str,
+    firmware: str = "",
+    can_interface: str = "",
+    operator: str = "",
+    approved_for_control: bool = False,
+) -> dict[str, Any]:
+    """Turn visually confirmed robot gesture captures into a runtime profile."""
+
+    profile = copy.deepcopy(
+        {
+            key: value
+            for key, value in seed_profile.items()
+            if not str(key).startswith("_profile_")
+        }
+    )
+    model = str(profile["model"]).upper()
+    length = MODEL_COMMAND_LENGTHS[model]
+    required = set(profile["required_poses"])
+    missing = required - set(captures)
+    if missing:
+        raise ValueError(f"missing {model} robot calibration poses: {sorted(missing)}")
+    pose_commands: dict[str, list[float]] = {}
+    poses: dict[str, Any] = {}
+    command_bounds = np.asarray(profile["command_bounds"], dtype=float)
+    for pose_name in profile["required_poses"]:
+        capture = captures[pose_name]
+        command = np.asarray(capture["cmd_u8"], dtype=float)
+        state = np.asarray(capture.get("state_u8", command), dtype=float)
+        if command.shape != (length,) or state.shape != (length,):
+            raise ValueError(f"{pose_name} must contain {length} command/state values")
+        if (
+            not np.all(np.isfinite(command))
+            or np.any(command < 0)
+            or np.any(command > 255)
+            or not np.all(np.isfinite(state))
+            or np.any(state < 0)
+            or np.any(state > 255)
+        ):
+            raise ValueError(f"{pose_name} contains an invalid command")
+        if np.any(command < command_bounds[:, 0]) or np.any(
+            command > command_bounds[:, 1]
+        ):
+            raise ValueError(
+                f"{pose_name} command exceeds the profile safety bounds"
+            )
+        pose_commands[pose_name] = command.astype(float).tolist()
+        status = str(capture.get("status", ""))
+        if status not in {"exact", "approximate", "unsupported"}:
+            raise ValueError(f"{pose_name} has an invalid status")
+        state_names = [str(name) for name in capture.get("state_names", ())]
+        if (
+            len(state_names) != length
+            or len(set(state_names)) != length
+            or state_names != list(profile["command_names"])
+        ):
+            raise ValueError(f"{pose_name} has invalid state names")
+        poses[pose_name] = {
+            "cmd_u8": command.astype(float).tolist(),
+            "command_names": list(profile["command_names"]),
+            "state_u8": state.astype(float).tolist(),
+            "state_names": state_names,
+            "status": status,
+            "confirmed": bool(capture.get("confirmed", False)),
+            "captured_at": str(capture.get("captured_at", "")),
+        }
+
+    for channel in profile["channels"]:
+        channel["sources"] = _fit_source_weights(
+            profile, channel, pose_commands
+        )
+        points = []
+        point_poses = channel.get("point_poses", ("open_spread", "fist"))
+        for pose_name in point_poses:
+            if pose_name in pose_commands:
+                sources = channel["sources"]
+                activation = sum(
+                    float(weight) * gesture_intent_value(pose_name, source)
+                    for source, weight in sources.items()
+                ) / max(sum(float(value) for value in sources.values()), 1e-9)
+                points.append(
+                    [
+                        float(activation),
+                        float(pose_commands[pose_name][int(channel["index"])]),
+                    ]
+                )
+        if channel.get("mapping") == "monotonic_piecewise":
+            channel["points"] = _monotonic_points(points)
+        else:
+            channel["points"] = _aggregate_points(points)
+        channel.pop("point_poses", None)
+
+    # Older seeds carried a discrete pinch selector configuration.  Preserve
+    # compatibility when reading those seeds, but never copy that mechanism
+    # into a newly calibrated continuous profile.  The named pinch captures
+    # already live in ``poses`` and are paired with the glove at runtime.
+    profile.pop("pinch", None)
+    profile["mapping_strategy"] = "paired_continuous_v1"
+    profile["poses"] = poses
+    profile["approved_for_control"] = (
+        bool(approved_for_control)
+        and bool(str(serial_number).strip())
+        and bool(str(can_interface).strip())
+        and bool(str(operator).strip())
+        and all(pose["confirmed"] for pose in poses.values())
+    )
+    profile["calibration"] = "provisional"
+    profile["created_at"] = datetime.now(timezone.utc).isoformat()
+    profile["device"] = {
+        "serial_number": str(serial_number),
+        "firmware": str(firmware),
+        "can_interface": str(can_interface),
+    }
+    profile.setdefault("metadata", {})
+    profile["metadata"]["operator_confirmed"] = bool(
+        profile["approved_for_control"]
+    )
+    profile["metadata"]["description"] = (
+        f"{model}左手机身多手势标定profile；无真实角度GT"
+    )
+    profile["metadata"]["operator"] = str(operator)
+    profile.pop("required_poses", None)
+    profile.pop("fit_sources", None)
+    profile["schema_version"] = SCHEMA_VERSION
+    profile.pop("kind", None)
+    profile["profile_kind"] = "robot_hand"
+    profile["side"] = "left"
+    return profile

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/constants.py

@@ -0,0 +1,162 @@
+"""Stable names and calibration pose definitions for linkerforce_v2."""
+
+from __future__ import annotations
+
+SCHEMA_VERSION = 1
+
+# Runtime defaults intentionally match the effective legacy left-G20 path:
+# direct latest-frame mapping and firmware-side interpolation.
+DEFAULT_INPUT_FILTER_ENABLED = False
+DEFAULT_COMMAND_FILTER_CONFIG = {
+    "mode": "passthrough",
+    "ema_alpha": 1.0,
+    "max_step_u8": 255.0,
+    "max_acceleration_u8_per_frame2": 255.0,
+    "deadband_u8": 0.0,
+}
+
+FFG_JOINT_NAMES = tuple(
+    [f"thumb_{index}" for index in range(5)]
+    + [f"index_{index}" for index in range(4)]
+    + [f"middle_{index}" for index in range(4)]
+    + [f"ring_{index}" for index in range(4)]
+    + [f"pinky_{index}" for index in range(4)]
+)
+
+BASE_INTENT_NAMES = (
+    "thumb_rotate",
+    "thumb_abduction",
+    "thumb_opposition",
+    "thumb_root",
+    "thumb_tip",
+    "index_root",
+    "index_tip",
+    "middle_root",
+    "middle_tip",
+    "ring_root",
+    "ring_tip",
+    "pinky_root",
+    "pinky_tip",
+    "index_splay",
+    "middle_splay",
+    "ring_splay",
+    "pinky_splay",
+)
+
+PINCH_ROLES = ("index", "middle", "ring", "pinky")
+PINCH_INTENT_NAMES = tuple(f"pinch_{role}" for role in PINCH_ROLES)
+INTENT_NAMES = BASE_INTENT_NAMES + PINCH_INTENT_NAMES + ("power_grasp",)
+
+GLOVE_STATIC_GESTURES = (
+    "open_spread",
+    "fingers_together",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky",
+)
+
+GLOVE_DYNAMIC_GESTURES = (
+    "index_flex_sweep",
+    "middle_flex_sweep",
+    "ring_flex_sweep",
+    "pinky_flex_sweep",
+    "thumb_flex_sweep",
+    "thumb_opposition_sweep",
+    "finger_splay_sweep",
+)
+
+G20_GESTURES = GLOVE_STATIC_GESTURES
+O6_GESTURES = (
+    "open_spread",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky",
+)
+
+GESTURE_STATUS_VALUES = ("exact", "approximate", "unsupported")
+
+MODEL_COMMAND_LENGTHS = {"G20": 20, "O6": 6}
+
+# Expected semantic endpoints are used only while generating a profile. Runtime
+# mapping consumes generated coefficients and does not contain gesture magic.
+GESTURE_INTENT_TARGETS = {
+    "open_spread": {
+        **{name: 0.0 for name in BASE_INTENT_NAMES},
+        "thumb_rotate": 0.5,
+        "thumb_abduction": 0.5,
+        "index_splay": 1.0,
+        "middle_splay": 1.0,
+        "ring_splay": 1.0,
+        "pinky_splay": 1.0,
+    },
+    "fingers_together": {
+        **{name: 0.0 for name in BASE_INTENT_NAMES},
+    },
+    "tabletop": {
+        "index_root": 1.0,
+        "middle_root": 1.0,
+        "ring_root": 1.0,
+        "pinky_root": 1.0,
+    },
+    "hook": {
+        "index_tip": 1.0,
+        "middle_tip": 1.0,
+        "ring_tip": 1.0,
+        "pinky_tip": 1.0,
+    },
+    "fist": {
+        "thumb_root": 1.0,
+        "thumb_tip": 1.0,
+        "thumb_opposition": 1.0,
+        "index_root": 1.0,
+        "index_tip": 1.0,
+        "middle_root": 1.0,
+        "middle_tip": 1.0,
+        "ring_root": 1.0,
+        "ring_tip": 1.0,
+        "pinky_root": 1.0,
+        "pinky_tip": 1.0,
+    },
+    "thumb_max_out": {
+        "thumb_rotate": 0.0,
+        "thumb_abduction": 0.0,
+        "thumb_opposition": 0.0,
+    },
+    "thumb_across": {
+        "thumb_rotate": 1.0,
+        "thumb_abduction": 1.0,
+        "thumb_opposition": 1.0,
+        "thumb_root": 0.75,
+        "thumb_tip": 0.75,
+    },
+}
+
+for _role in PINCH_ROLES:
+    GESTURE_INTENT_TARGETS[f"pinch_{_role}"] = {
+        "thumb_rotate": 0.8,
+        "thumb_abduction": 0.8,
+        "thumb_opposition": 1.0,
+        "thumb_root": 0.75,
+        "thumb_tip": 0.85,
+        f"{_role}_root": 0.55,
+        f"{_role}_tip": 0.65,
+        f"pinch_{_role}": 1.0,
+    }
+
+
+def gesture_intent_value(gesture: str, feature: str) -> float:
+    """Return the canonical calibration target for a gesture/feature pair."""
+
+    return float(GESTURE_INTENT_TARGETS.get(gesture, {}).get(feature, 0.0))

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/node.py

@@ -0,0 +1,866 @@
+"""ROS 2 node for one left FFG driving G20 and O6 through profiles."""
+
+from __future__ import annotations
+
+from collections import deque
+from datetime import datetime, timezone
+import json
+import math
+from pathlib import Path
+import time
+from typing import Any
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import String
+from std_srvs.srv import SetBool, Trigger
+
+from ...linkerhand.constants import HandType
+from ...linkerhand.linkerforce import ForceSerialReader
+from .constants import (
+    DEFAULT_COMMAND_FILTER_CONFIG,
+    DEFAULT_INPUT_FILTER_ENABLED,
+    FFG_JOINT_NAMES,
+    INTENT_NAMES,
+    MODEL_COMMAND_LENGTHS,
+)
+from .mapping import (
+    HandIntentExtractor,
+    MappingResult,
+    RobotMapper,
+    VectorKalmanFilter,
+)
+from .profiles import ProfileError, load_glove_profile, load_robot_profile
+from .safety import evaluate_timeouts
+
+
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+class FFGDualRetargetNode(Node):
+    """Profile-driven, fail-closed retargeting for a single left FFG."""
+
+    def __init__(self) -> None:
+        super().__init__("ffg_dual_retarget")
+        self._declare_parameters()
+
+        self.publish_rate = float(self.get_parameter("publish_rate").value)
+        self.glove_timeout = float(self.get_parameter("glove_timeout").value)
+        self.driver_timeout = float(self.get_parameter("driver_timeout").value)
+        if (
+            self.publish_rate <= 0.0
+            or self.glove_timeout <= 0.0
+            or self.driver_timeout <= 0.0
+        ):
+            raise ValueError("publish_rate and timeout parameters must be positive")
+        self.input_mode = str(self.get_parameter("input_mode").value).strip().lower()
+        self.input_filter_enabled = bool(
+            self.get_parameter("input_filter_enabled").value
+        )
+        self.input_filter_process_variance = float(
+            self.get_parameter("input_filter_process_variance").value
+        )
+        self.input_filter_measurement_variance = float(
+            self.get_parameter("input_filter_measurement_variance").value
+        )
+        self.input_filter_reset_gap = float(
+            self.get_parameter("input_filter_reset_gap").value
+        )
+        self.command_filter_config = {
+            "mode": str(
+                self.get_parameter("command_filter_mode").value
+            ).strip().lower(),
+            "ema_alpha": float(
+                self.get_parameter("command_filter_ema_alpha").value
+            ),
+            "max_step_u8": float(
+                self.get_parameter("command_filter_max_step_u8").value
+            ),
+            "max_acceleration_u8_per_frame2": float(
+                self.get_parameter("command_filter_max_step_u8").value
+            ),
+            "deadband_u8": float(
+                self.get_parameter("command_filter_deadband_u8").value
+            ),
+        }
+        self.sequence = 0
+        self.last_glove_time = 0.0
+        self.last_processed_sequence = -1
+        self.latest_raw: list[float] | None = None
+        self.latest_intent: dict[str, float] | None = None
+        self.latencies_ms: deque[float] = deque(maxlen=600)
+        self.last_status_publish = 0.0
+        self.last_fault = ""
+
+        self.glove_profile: dict[str, Any] | None = None
+        self.wear_check_valid = False
+        self.intent_extractor: HandIntentExtractor | None = None
+        self.profile_errors: dict[str, str] = {}
+        self.robot_profiles: dict[str, dict[str, Any]] = {}
+        self.mappers: dict[str, RobotMapper] = {}
+        self.last_anchor_weights: dict[
+            str,
+            tuple[tuple[str, float], ...],
+        ] = {
+            "G20": (),
+            "O6": (),
+        }
+        self.enabled = {"G20": False, "O6": False}
+        self.driver_state_time = {"G20": 0.0, "O6": 0.0}
+        self.driver_state_valid = {"G20": False, "O6": False}
+        self.latest_driver_state: dict[str, tuple[float, ...] | None] = {
+            "G20": None,
+            "O6": None,
+        }
+        self.last_hardware_command: dict[str, tuple[int, ...] | None] = {
+            "G20": None,
+            "O6": None,
+        }
+        self.serial_reader: ForceSerialReader | None = None
+        self.input_filter = VectorKalmanFilter(
+            len(FFG_JOINT_NAMES),
+            process_variance=self.input_filter_process_variance,
+            measurement_variance=self.input_filter_measurement_variance,
+            reset_gap_s=self.input_filter_reset_gap,
+        )
+
+        self.raw_publisher = self.create_publisher(
+            JointState, "/ffg/left/raw_joint_state", 10
+        )
+        self.filtered_raw_publisher = self.create_publisher(
+            JointState, "/ffg/left/filtered_joint_state", 10
+        )
+        self.intent_publisher = self.create_publisher(
+            JointState, "/retarget/left/hand_intent", 10
+        )
+        self.frame_meta_publisher = self.create_publisher(
+            String, "/retarget/left/frame_meta", 10
+        )
+        self.status_publisher = self.create_publisher(String, "~/status", 10)
+
+        self.actuation_publishers: dict[str, Any] = {}
+        self.nominal_publishers: dict[str, Any] = {}
+        self.preview_publishers: dict[str, Any] = {}
+        self.command_publishers: dict[str, Any] = {}
+        self.state_subscriptions: list[Any] = []
+        for model in ("G20", "O6"):
+            lower = model.lower()
+            self.actuation_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/actuation_target", 10
+            )
+            self.nominal_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/joint_target_nominal", 10
+            )
+            self.preview_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/cmd_u8_preview", 10
+            )
+            self.command_publishers[model] = self.create_publisher(
+                JointState, f"/{lower}/cb_left_hand_control_cmd", COMMAND_QOS
+            )
+            self.state_subscriptions.append(
+                self.create_subscription(
+                    JointState,
+                    f"/{lower}/cb_left_hand_state",
+                    lambda message, selected=model: self._state_callback(
+                        selected, message
+                    ),
+                    10,
+                )
+            )
+
+        self.create_service(
+            SetBool,
+            "~/enable_g20",
+            lambda request, response: self._enable_callback(
+                "G20", request, response
+            ),
+        )
+        self.create_service(
+            SetBool,
+            "~/enable_o6",
+            lambda request, response: self._enable_callback(
+                "O6", request, response
+            ),
+        )
+        self.create_service(SetBool, "~/enable_all", self._enable_all_callback)
+        self.create_service(Trigger, "~/emergency_stop", self._estop_callback)
+
+        self._load_profiles()
+        if self.input_mode == "topic":
+            raw_topic = str(self.get_parameter("raw_input_topic").value)
+            self.raw_subscription = self.create_subscription(
+                JointState, raw_topic, self._raw_topic_callback, 10
+            )
+            self.get_logger().info(f"FFG输入使用ROS话题: {raw_topic}")
+        elif self.input_mode == "serial":
+            self._connect_left_glove()
+        else:
+            raise ValueError("input_mode must be 'serial' or 'topic'")
+
+        self.process_timer = self.create_timer(
+            1.0 / max(self.publish_rate, 1.0), self._process_latest
+        )
+        self.watchdog_timer = self.create_timer(0.05, self._watchdog)
+        self.get_logger().info(
+            "linkerforce_v2已启动：默认PREVIEW，单只左手套即可运行；"
+            "G20/O6必须分别加载匹配且已批准的实机profile后才能使能"
+        )
+
+    def _declare_parameters(self) -> None:
+        self.declare_parameter("input_mode", "serial")
+        self.declare_parameter("raw_input_topic", "/ffg/left/raw_joint_state")
+        self.declare_parameter("serial_port", "")
+        self.declare_parameter("baudrate", 0)
+        self.declare_parameter("baudrates", [2000000, 460800, 1000000, 921600])
+        self.declare_parameter("auto_scan", True)
+        self.declare_parameter("serial_debug", False)
+        self.declare_parameter("glove_profile", "")
+        self.declare_parameter("wear_check", "")
+        self.declare_parameter("wear_check_max_age_hours", 12.0)
+        self.declare_parameter("g20_profile", "")
+        self.declare_parameter("o6_profile", "")
+        self.declare_parameter("g20_serial_number", "")
+        self.declare_parameter("o6_serial_number", "")
+        self.declare_parameter("g20_can_interface", "can0")
+        self.declare_parameter("o6_can_interface", "can1")
+        self.declare_parameter("publish_rate", 30.0)
+        self.declare_parameter("glove_timeout", 0.35)
+        self.declare_parameter("driver_timeout", 1.0)
+        # The legacy left-G20 runtime consumes the latest 21-value glove frame
+        # directly.  Keep that timing by default; Kalman remains opt-in for a
+        # particularly noisy glove.
+        self.declare_parameter(
+            "input_filter_enabled", DEFAULT_INPUT_FILTER_ENABLED
+        )
+        self.declare_parameter("input_filter_process_variance", 1e-5)
+        self.declare_parameter("input_filter_measurement_variance", 5e-4)
+        self.declare_parameter("input_filter_reset_gap", 0.35)
+        # LeftHand.joint_update() in the legacy G20 mapper did not call its
+        # declared _apply_smooth() helper.  Pass the calibrated target straight
+        # to the G20 firmware as it did, avoiding a second sample-and-hold layer.
+        self.declare_parameter(
+            "command_filter_mode",
+            DEFAULT_COMMAND_FILTER_CONFIG["mode"],
+        )
+        self.declare_parameter(
+            "command_filter_ema_alpha",
+            DEFAULT_COMMAND_FILTER_CONFIG["ema_alpha"],
+        )
+        self.declare_parameter(
+            "command_filter_max_step_u8",
+            DEFAULT_COMMAND_FILTER_CONFIG["max_step_u8"],
+        )
+        self.declare_parameter(
+            "command_filter_deadband_u8",
+            DEFAULT_COMMAND_FILTER_CONFIG["deadband_u8"],
+        )
+
+    def _load_profiles(self) -> None:
+        glove_path = str(self.get_parameter("glove_profile").value).strip()
+        if glove_path:
+            try:
+                self.glove_profile = load_glove_profile(glove_path)
+                self.intent_extractor = HandIntentExtractor(self.glove_profile)
+                self.get_logger().info(
+                    "FFG profile已加载: "
+                    f"{Path(glove_path).name} "
+                    f"sha256={self.glove_profile['_profile_sha256'][:12]}"
+                )
+                self._load_wear_check()
+            except (ProfileError, TypeError, ValueError) as exc:
+                self.profile_errors["FFG"] = str(exc)
+                self.get_logger().error(f"FFG profile无效，仅保留raw预览: {exc}")
+        else:
+            self.profile_errors["FFG"] = "glove_profile未配置"
+            self.get_logger().warning("未配置FFG profile，仅发布原始手套数据")
+
+        for model in ("G20", "O6"):
+            parameter = f"{model.lower()}_profile"
+            path = str(self.get_parameter(parameter).value).strip()
+            if not path:
+                self.profile_errors[model] = f"{parameter}未配置"
+                self.get_logger().warning(
+                    f"未配置{model} profile，不生成该型号目标"
+                )
+                continue
+            try:
+                profile = load_robot_profile(path, model)
+                self.robot_profiles[model] = profile
+                self.mappers[model] = RobotMapper(
+                    profile,
+                    self.glove_profile,
+                    command_filter_config=self.command_filter_config,
+                )
+                mode = (
+                    "可申请实机使能"
+                    if profile.get("approved_for_control")
+                    else "仅预览"
+                )
+                self.get_logger().info(
+                    f"{model} profile已加载({mode}): {Path(path).name} "
+                    f"sha256={profile['_profile_sha256'][:12]}"
+                )
+                mapper = self.mappers[model]
+                self.get_logger().info(
+                    f"{model}映射={mapper.mapping_mode}, "
+                    "执行滤波="
+                    f"{mapper.command_filter_config['mode']} "
+                    f"alpha={mapper.command_filter_config['ema_alpha']}, "
+                    "max_step="
+                    f"{mapper.command_filter_config['max_step_u8']}"
+                )
+            except (ProfileError, TypeError, ValueError) as exc:
+                self.profile_errors[model] = str(exc)
+                self.get_logger().error(f"{model} profile无效: {exc}")
+
+    def _load_wear_check(self) -> None:
+        path = str(self.get_parameter("wear_check").value).strip()
+        if not path:
+            self.profile_errors["wear_check"] = "wear_check未配置"
+            self.get_logger().warning(
+                "未配置三姿势快速佩戴检查凭据，实机使能将被拒绝"
+            )
+            return
+        try:
+            data = json.loads(Path(path).expanduser().resolve().read_text("utf-8"))
+            checked_at = datetime.fromisoformat(str(data["checked_at"]))
+            if checked_at.tzinfo is None:
+                checked_at = checked_at.replace(tzinfo=timezone.utc)
+            age_hours = (
+                datetime.now(timezone.utc) - checked_at.astimezone(timezone.utc)
+            ).total_seconds() / 3600.0
+            expected_hash = (
+                self.glove_profile.get("_profile_sha256")
+                if self.glove_profile
+                else None
+            )
+            if data.get("kind") != "ffg_wear_check" or not data.get("passed"):
+                raise ValueError("快速佩戴检查未通过")
+            if data.get("glove_profile_sha256") != expected_hash:
+                raise ValueError("快速佩戴检查与当前FFG profile哈希不匹配")
+            maximum = float(
+                self.get_parameter("wear_check_max_age_hours").value
+            )
+            if age_hours < 0.0 or age_hours > maximum:
+                raise ValueError(
+                    f"快速佩戴检查已过期: {age_hours:.1f}h > {maximum:.1f}h"
+                )
+            self.wear_check_valid = True
+            self.get_logger().info(
+                f"三姿势快速佩戴检查有效，凭据年龄{age_hours:.2f}h"
+            )
+        except (OSError, KeyError, TypeError, ValueError, json.JSONDecodeError) as exc:
+            self.profile_errors["wear_check"] = str(exc)
+            self.get_logger().error(f"快速佩戴检查凭据无效: {exc}")
+
+    def _log_from_reader(self, level: str, message: str) -> None:
+        logger = self.get_logger()
+        method = getattr(logger, "warning" if level == "warn" else level, logger.info)
+        method(message)
+
+    def _connect_left_glove(self) -> None:
+        configured_rates = [
+            int(value) for value in self.get_parameter("baudrates").value
+        ]
+        requested_rate = int(self.get_parameter("baudrate").value)
+        if requested_rate > 0:
+            configured_rates = [requested_rate] + [
+                value for value in configured_rates if value != requested_rate
+            ]
+        reader = ForceSerialReader(
+            HandType.left,
+            baudrates=configured_rates,
+            isdebug=bool(self.get_parameter("serial_debug").value),
+            logger=self._log_from_reader,
+        )
+        configured_port = str(self.get_parameter("serial_port").value).strip()
+        auto_scan = bool(self.get_parameter("auto_scan").value)
+        port: str | None = None
+        baud: int | None = None
+        if configured_port:
+            success, detected_baud, error = reader.query_serial_port(
+                configured_port, timeout=1.0
+            )
+            if success:
+                port, baud = configured_port, int(detected_baud)
+            else:
+                self.get_logger().error(
+                    f"左手套端口检测失败: {configured_port}, error={error}"
+                )
+        if port is None and auto_scan:
+            detected_port, detected_baud, _ = reader.find_valid_ports(timeout=1.0)
+            if detected_port:
+                port, baud = str(detected_port), int(detected_baud)
+        if port is None or baud is None:
+            self.get_logger().error(
+                "未找到左手FFG；节点保持运行并禁止实机控制，不会寻找右手套"
+            )
+            return
+        if not reader.openserial(port, baud):
+            self.get_logger().error(f"无法打开左手FFG: {port} @ {baud}")
+            return
+        reader.start()
+        self.serial_reader = reader
+        self.get_logger().info(
+            f"左手FFG已连接: {port} @ {baud}, version={reader.version or 'unknown'}"
+        )
+
+    def _raw_topic_callback(self, message: JointState) -> None:
+        if len(message.position) != len(FFG_JOINT_NAMES):
+            self.last_fault = (
+                f"FFG topic长度错误: {len(message.position)}, 应为21"
+            )
+            return
+        names = [str(name) for name in message.name]
+        if len(names) != 21 or set(names) != set(FFG_JOINT_NAMES):
+            self.last_fault = "FFG topic关节名称缺失、重复或不匹配"
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(names, message.position)
+        }
+        raw = [lookup[name] for name in FFG_JOINT_NAMES]
+        if not all(math.isfinite(value) for value in raw):
+            self.last_fault = "FFG topic包含非有限值"
+            return
+        self.sequence += 1
+        self.latest_raw = raw
+        self.last_glove_time = time.monotonic()
+
+    def _state_callback(self, model: str, message: JointState) -> None:
+        values = [float(value) for value in message.position]
+        names = [str(name) for name in message.name]
+        expected_names = list(
+            self.robot_profiles.get(model, {}).get("command_names", ())
+        )
+        valid = (
+            len(values) == MODEL_COMMAND_LENGTHS[model]
+            and all(math.isfinite(value) for value in values)
+            and all(0.0 <= value <= 255.0 for value in values)
+            and names == expected_names
+        )
+        self.driver_state_valid[model] = valid
+        if valid:
+            self.driver_state_time[model] = time.monotonic()
+            self.latest_driver_state[model] = tuple(values)
+
+    def _update_serial_frame(self) -> None:
+        if self.serial_reader is None:
+            return
+        raw, frame_sequence, received_at = self.serial_reader.position_snapshot
+        if frame_sequence == self.last_processed_sequence or frame_sequence <= 0:
+            return
+        self.last_processed_sequence = frame_sequence
+        if len(raw) != len(FFG_JOINT_NAMES) or not np.all(np.isfinite(raw)):
+            self.last_fault = "串口FFG帧不是21维有限值"
+            return
+        self.sequence += 1
+        self.latest_raw = [float(value) for value in raw]
+        self.last_glove_time = float(received_at)
+
+    def _joint_state(
+        self,
+        names: list[str] | tuple[str, ...],
+        values: Any,
+        stamp: Any | None = None,
+    ) -> JointState:
+        message = JointState()
+        message.header.stamp = stamp or self.get_clock().now().to_msg()
+        message.name = list(names)
+        message.position = [float(value) for value in values]
+        return message
+
+    def _publish_mapping(
+        self,
+        model: str,
+        result: MappingResult,
+        stamp: Any,
+    ) -> None:
+        self.actuation_publishers[model].publish(
+            self._joint_state(result.actuation_names, result.actuation, stamp)
+        )
+        self.nominal_publishers[model].publish(
+            self._joint_state(result.sim_joint_names, result.q_nominal, stamp)
+        )
+        names = self.robot_profiles[model]["command_names"]
+        command_message = self._joint_state(names, result.command, stamp)
+        self.preview_publishers[model].publish(command_message)
+        self.last_anchor_weights[model] = result.anchor_weights
+        if self.enabled[model]:
+            self.command_publishers[model].publish(command_message)
+            self.last_hardware_command[model] = result.command
+
+    def _publish_frame_meta(self, stamp: Any) -> None:
+        message = String()
+        message.data = json.dumps(
+            {
+                "timestamp_ns": int(stamp.sec) * 1_000_000_000
+                + int(stamp.nanosec),
+                "sequence": self.sequence,
+                "calibration": "provisional",
+                "q_gt": None,
+            },
+            ensure_ascii=False,
+            sort_keys=True,
+        )
+        self.frame_meta_publisher.publish(message)
+
+    def _process_latest(self) -> None:
+        if self.input_mode == "serial":
+            self._update_serial_frame()
+        raw = self.latest_raw
+        if raw is None:
+            return
+        new_frame = self.sequence != getattr(self, "_mapped_sequence", -1)
+        frame_stamp = self.get_clock().now().to_msg()
+        try:
+            if new_frame:
+                self._mapped_sequence = self.sequence
+                if self.input_mode == "serial":
+                    self.raw_publisher.publish(
+                        self._joint_state(
+                            FFG_JOINT_NAMES,
+                            raw,
+                            frame_stamp,
+                        )
+                    )
+                if self.intent_extractor is None:
+                    return
+                filtered_raw = (
+                    self.input_filter.apply(
+                        raw,
+                        timestamp_s=self.last_glove_time,
+                    )
+                    if self.input_filter_enabled
+                    else np.asarray(raw, dtype=float)
+                )
+                self.filtered_raw_publisher.publish(
+                    self._joint_state(
+                        FFG_JOINT_NAMES,
+                        filtered_raw,
+                        frame_stamp,
+                    )
+                )
+                self.latest_intent = self.intent_extractor.extract(
+                    filtered_raw
+                )
+                self.intent_publisher.publish(
+                    self._joint_state(
+                        INTENT_NAMES,
+                        [
+                            self.latest_intent[name]
+                            for name in INTENT_NAMES
+                        ],
+                        frame_stamp,
+                    )
+                )
+                self._publish_frame_meta(frame_stamp)
+                if self.last_glove_time > 0.0:
+                    self.latencies_ms.append(
+                        max(
+                            0.0,
+                            (
+                                time.monotonic()
+                                - self.last_glove_time
+                            )
+                            * 1000.0,
+                        )
+                    )
+
+            # Advance and publish the motor trajectory on every timer tick.
+            # Reusing the most recent valid intent is intentional: the legacy
+            # path also ran its controller at a fixed 30 Hz even between
+            # serial updates.  The watchdog still revokes real control when
+            # the source becomes stale.
+            if self.latest_intent is None:
+                return
+            for model, mapper in self.mappers.items():
+                self._publish_mapping(
+                    model,
+                    mapper.map(self.latest_intent),
+                    frame_stamp,
+                )
+        except (ValueError, FloatingPointError) as exc:
+            self.last_fault = f"映射失败: {exc}"
+            self._disable_all(self.last_fault)
+
+    def _identity_error(self, model: str) -> str | None:
+        profile = self.robot_profiles.get(model)
+        if profile is None:
+            return "profile未加载"
+        if not profile.get("approved_for_control", False):
+            return "profile未获实机控制批准"
+        device = profile.get("device")
+        if not isinstance(device, dict):
+            return "profile缺少设备SN/CAN元数据"
+        expected_sn = str(
+            self.get_parameter(f"{model.lower()}_serial_number").value
+        ).strip()
+        profile_sn = str(device.get("serial_number", "")).strip()
+        if not expected_sn:
+            return f"未设置{model.lower()}_serial_number，无法核对单机profile"
+        if profile_sn != expected_sn:
+            return f"profile SN={profile_sn} 与期望SN={expected_sn}不匹配"
+        expected_can = str(
+            self.get_parameter(f"{model.lower()}_can_interface").value
+        ).strip()
+        if str(device.get("can_interface", "")).strip() != expected_can:
+            return "profile CAN接口与启动配置不匹配"
+        return None
+
+    def _can_enable(self, model: str) -> str | None:
+        if (
+            self.glove_profile is None
+            or not self.glove_profile.get("approved_for_runtime", False)
+        ):
+            return "FFG profile采样或动态轨迹不完整，仅允许预览"
+        if not self.wear_check_valid:
+            return "三姿势快速佩戴检查缺失、失败或过期"
+        identity_error = self._identity_error(model)
+        if identity_error:
+            return identity_error
+        if self.mappers[model].mapping_mode != "factorized_paired_v2":
+            return "未建立FFG与机械手分解式姿势映射，仅允许预览"
+        now = time.monotonic()
+        if self.last_glove_time <= 0.0 or now - self.last_glove_time > self.glove_timeout:
+            return "FFG无新帧或已超时"
+        if (
+            not self.driver_state_valid[model]
+            or now - self.driver_state_time[model] > self.driver_timeout
+        ):
+            return f"{model}驱动状态无效或已超时"
+        return None
+
+    def _enable_callback(
+        self, model: str, request: SetBool.Request, response: SetBool.Response
+    ) -> SetBool.Response:
+        if not request.data:
+            self.enabled[model] = False
+            response.success = True
+            response.message = f"{model}已停用"
+            return response
+        error = self._can_enable(model)
+        if error:
+            response.success = False
+            response.message = f"{model}拒绝使能: {error}"
+            return response
+        self.mappers[model].reset(self.latest_driver_state[model])
+        self.enabled[model] = True
+        response.success = True
+        response.message = f"{model}已使能"
+        return response
+
+    def _enable_all_callback(
+        self, request: SetBool.Request, response: SetBool.Response
+    ) -> SetBool.Response:
+        if not request.data:
+            self._disable_all("用户停用全部")
+            response.success = True
+            response.message = "G20/O6均已停用"
+            return response
+        errors = {
+            model: error
+            for model in ("G20", "O6")
+            if (error := self._can_enable(model)) is not None
+        }
+        if errors:
+            response.success = False
+            response.message = "拒绝全部使能: " + "; ".join(
+                f"{model}={error}" for model, error in errors.items()
+            )
+            return response
+        for model in ("G20", "O6"):
+            self.mappers[model].reset(self.latest_driver_state[model])
+            self.enabled[model] = True
+        response.success = True
+        response.message = "G20/O6均已使能"
+        return response
+
+    def _disable_all(self, reason: str) -> None:
+        was_enabled = any(self.enabled.values())
+        self.enabled = {"G20": False, "O6": False}
+        self.last_fault = reason
+        if was_enabled:
+            self.get_logger().error(f"实机控制已全部撤销: {reason}")
+
+    def _estop_callback(
+        self, request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        del request
+        self._disable_all("软件急停")
+        response.success = True
+        response.message = "软件急停已触发；命令保持在最后值，需重新显式使能"
+        return response
+
+    def _watchdog(self) -> None:
+        now = time.monotonic()
+        disable_all, timed_out_models = evaluate_timeouts(
+            now=now,
+            enabled=self.enabled,
+            last_glove_time=self.last_glove_time,
+            driver_state_time=self.driver_state_time,
+            driver_state_valid=self.driver_state_valid,
+            glove_timeout=self.glove_timeout,
+            driver_timeout=self.driver_timeout,
+        )
+        if disable_all:
+            self._disable_all(f"FFG超过{self.glove_timeout:.2f}s无新帧")
+        for model in timed_out_models:
+            self.enabled[model] = False
+            self.last_fault = f"{model}驱动状态超过{self.driver_timeout:.2f}s未更新"
+            self.get_logger().error(
+                f"{model}实机控制已撤销，另一型号不受影响: {self.last_fault}"
+            )
+        if now - self.last_status_publish >= 1.0:
+            self._publish_status(now)
+            self.last_status_publish = now
+
+    def _publish_status(self, now: float) -> None:
+        latency_p95 = (
+            float(np.percentile(np.asarray(self.latencies_ms), 95))
+            if self.latencies_ms
+            else None
+        )
+        profile_hashes = {
+            "glove": (
+                self.glove_profile.get("_profile_sha256")
+                if self.glove_profile
+                else None
+            ),
+            **{
+                model.lower(): (
+                    self.robot_profiles[model].get("_profile_sha256")
+                    if model in self.robot_profiles
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+        }
+        payload = {
+            "timestamp_ns": int(self.get_clock().now().nanoseconds),
+            "sequence": self.sequence,
+            "calibration": "provisional",
+            "q_gt": None,
+            "input_mode": self.input_mode,
+            "glove_age_s": (
+                None
+                if self.last_glove_time <= 0.0
+                else round(now - self.last_glove_time, 4)
+            ),
+            "enabled": {key.lower(): value for key, value in self.enabled.items()},
+            "profile_approved": {
+                "glove": bool(
+                    self.glove_profile
+                    and self.glove_profile.get("approved_for_runtime", False)
+                ),
+                **{
+                    model.lower(): bool(
+                        model in self.robot_profiles
+                        and self.robot_profiles[model].get(
+                            "approved_for_control", False
+                        )
+                    )
+                    for model in ("G20", "O6")
+                },
+            },
+            "mapping_mode": {
+                model.lower(): (
+                    self.mappers[model].mapping_mode
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "input_filter": {
+                "enabled": self.input_filter_enabled,
+                "type": "vector_kalman_v1",
+                "process_variance": self.input_filter_process_variance,
+                "measurement_variance": (
+                    self.input_filter_measurement_variance
+                ),
+                "reset_gap_s": self.input_filter_reset_gap,
+            },
+            "command_filter": {
+                model.lower(): (
+                    self.mappers[model].command_filter_config
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "simulation_mapping_mode": {
+                model.lower(): (
+                    self.mappers[model].simulation_mapping_mode
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "anchor_weights": {
+                model.lower(): [
+                    {
+                        "pose": pose,
+                        "weight": round(weight, 6),
+                    }
+                    for pose, weight in self.last_anchor_weights[model][:5]
+                ]
+                for model in ("G20", "O6")
+            },
+            "wear_check_valid": self.wear_check_valid,
+            "driver_state_age_s": {
+                model.lower(): (
+                    None
+                    if self.driver_state_time[model] <= 0.0
+                    else round(now - self.driver_state_time[model], 4)
+                )
+                for model in ("G20", "O6")
+            },
+            "profile_sha256": profile_hashes,
+            "urdf_sha256": {
+                model.lower(): (
+                    self.robot_profiles[model]["simulation"]["urdf_sha256"]
+                    if model in self.robot_profiles
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "profile_errors": self.profile_errors,
+            "latency_p95_ms": latency_p95,
+            "last_fault": self.last_fault or None,
+        }
+        message = String()
+        message.data = json.dumps(payload, ensure_ascii=False, sort_keys=True)
+        self.status_publisher.publish(message)
+
+    def destroy_node(self) -> bool:
+        if self.serial_reader is not None:
+            self.serial_reader.stop()
+            self.serial_reader = None
+        return super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: FFGDualRetargetNode | None = None
+    try:
+        node = FFGDualRetargetNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/profiles.py

@@ -0,0 +1,571 @@
+"""Strict JSON profile loading and validation for linkerforce_v2."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+from .constants import (
+    BASE_INTENT_NAMES,
+    FFG_JOINT_NAMES,
+    G20_GESTURES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+    MODEL_COMMAND_LENGTHS,
+    O6_GESTURES,
+    PINCH_ROLES,
+    SCHEMA_VERSION,
+)
+
+
+class ProfileError(ValueError):
+    """Raised when a calibration profile is missing or internally inconsistent."""
+
+
+def _finite_scalar(value: Any, *, field: str) -> float:
+    try:
+        result = float(value)
+    except (TypeError, ValueError) as exc:
+        raise ProfileError(f"{field} must be numeric") from exc
+    if not math.isfinite(result):
+        raise ProfileError(f"{field} must be finite")
+    return result
+
+
+def _finite_vector(
+    value: Any,
+    *,
+    length: int,
+    field: str,
+    minimum: float | None = None,
+    maximum: float | None = None,
+) -> list[float]:
+    if not isinstance(value, Sequence) or isinstance(value, (str, bytes)):
+        raise ProfileError(f"{field} must be an array")
+    if len(value) != length:
+        raise ProfileError(f"{field} must contain {length} values, got {len(value)}")
+    try:
+        result = [float(item) for item in value]
+    except (TypeError, ValueError) as exc:
+        raise ProfileError(f"{field} contains a non-numeric value") from exc
+    if not all(math.isfinite(item) for item in result):
+        raise ProfileError(f"{field} contains a non-finite value")
+    if minimum is not None and any(item < minimum for item in result):
+        raise ProfileError(f"{field} contains a value below {minimum}")
+    if maximum is not None and any(item > maximum for item in result):
+        raise ProfileError(f"{field} contains a value above {maximum}")
+    return result
+
+
+def _load_json(path: str | Path) -> dict[str, Any]:
+    resolved = Path(path).expanduser().resolve()
+    if not resolved.is_file():
+        raise ProfileError(f"profile does not exist: {resolved}")
+    try:
+        data = json.loads(resolved.read_text(encoding="utf-8"))
+    except (OSError, json.JSONDecodeError) as exc:
+        raise ProfileError(f"cannot read profile {resolved}: {exc}") from exc
+    if not isinstance(data, dict):
+        raise ProfileError(f"profile root must be an object: {resolved}")
+    data["_profile_path"] = str(resolved)
+    data["_profile_sha256"] = profile_sha256(data)
+    return data
+
+
+def profile_sha256(profile: Mapping[str, Any]) -> str:
+    """Hash only persistent profile content, excluding loader metadata."""
+
+    persistent = {
+        key: value for key, value in profile.items() if not key.startswith("_profile_")
+    }
+    payload = json.dumps(
+        persistent, ensure_ascii=False, sort_keys=True, separators=(",", ":")
+    ).encode("utf-8")
+    return hashlib.sha256(payload).hexdigest()
+
+
+def save_profile(profile: Mapping[str, Any], path: str | Path) -> Path:
+    """Atomically save a profile as deterministic UTF-8 JSON."""
+
+    resolved = Path(path).expanduser().resolve()
+    resolved.parent.mkdir(parents=True, exist_ok=True)
+    persistent = {
+        key: value for key, value in profile.items() if not key.startswith("_profile_")
+    }
+    temporary = resolved.with_suffix(resolved.suffix + ".tmp")
+    temporary.write_text(
+        json.dumps(persistent, ensure_ascii=False, indent=2, sort_keys=True) + "\n",
+        encoding="utf-8",
+    )
+    temporary.replace(resolved)
+    return resolved
+
+
+def load_glove_profile(path: str | Path) -> dict[str, Any]:
+    """Load and validate an FFG glove calibration profile."""
+
+    profile = _load_json(path)
+    if profile.get("schema_version") != SCHEMA_VERSION:
+        raise ProfileError("unsupported glove profile schema_version")
+    if profile.get("profile_kind", profile.get("kind")) != "ffg_glove":
+        raise ProfileError("profile_kind must be ffg_glove")
+    if profile.get("calibration") != "provisional":
+        raise ProfileError("glove profile calibration must be provisional")
+    glove = profile.get("glove")
+    if not isinstance(glove, dict) or glove.get("side") != "left":
+        raise ProfileError("glove profile must describe a left glove")
+    joint_names = profile.get("joint_names")
+    if list(joint_names or []) != list(FFG_JOINT_NAMES):
+        raise ProfileError("glove profile joint_names do not match the FFG v2 layout")
+    poses = profile.get("poses")
+    if not isinstance(poses, dict):
+        raise ProfileError("glove profile poses must be an object")
+    missing_poses = set(GLOVE_STATIC_GESTURES) - set(poses)
+    if missing_poses:
+        raise ProfileError(
+            f"glove profile is missing poses: {sorted(missing_poses)}"
+        )
+    for name in GLOVE_STATIC_GESTURES:
+        summary = poses[name]
+        if not isinstance(summary, dict):
+            raise ProfileError(f"poses.{name} must be an object")
+        try:
+            count = int(summary.get("sample_count", 0))
+        except (TypeError, ValueError) as exc:
+            raise ProfileError(f"poses.{name}.sample_count is invalid") from exc
+        raw_frames = summary.get("raw_frames")
+        if not isinstance(raw_frames, list) or len(raw_frames) != count:
+            raise ProfileError(f"poses.{name} raw frame count is inconsistent")
+        for frame_index, frame in enumerate(raw_frames):
+            _finite_vector(
+                frame,
+                length=21,
+                field=f"poses.{name}.raw_frames[{frame_index}]",
+            )
+        _finite_vector(summary.get("median"), length=21, field=f"{name}.median")
+        _finite_vector(summary.get("mad"), length=21, field=f"{name}.mad")
+
+    features = profile.get("features")
+    if not isinstance(features, dict):
+        raise ProfileError("glove profile features must be an object")
+    missing = set(BASE_INTENT_NAMES) - set(features)
+    if missing:
+        raise ProfileError(f"glove profile is missing features: {sorted(missing)}")
+    for name in BASE_INTENT_NAMES:
+        spec = features[name]
+        if not isinstance(spec, dict):
+            raise ProfileError(f"features.{name} must be an object")
+        indices = spec.get("indices")
+        if not isinstance(indices, list) or not indices:
+            raise ProfileError(f"features.{name}.indices must be non-empty")
+        if any(not isinstance(index, int) or index < 0 or index >= 21 for index in indices):
+            raise ProfileError(f"features.{name}.indices contains an invalid index")
+        weights = _finite_vector(
+            spec.get("weights"), length=len(indices), field=f"{name}.weights"
+        )
+        if any(weight < 0.0 for weight in weights) or not math.isclose(
+            sum(weights), 1.0, abs_tol=1e-6
+        ):
+            raise ProfileError(f"features.{name}.weights must be normalized")
+        _finite_vector(spec.get("low"), length=len(indices), field=f"{name}.low")
+        _finite_vector(spec.get("high"), length=len(indices), field=f"{name}.high")
+
+    pinch = profile.get("pinch_anchors")
+    if not isinstance(pinch, dict) or set(pinch) != set(PINCH_ROLES):
+        raise ProfileError("glove profile must contain four pinch_anchors")
+    for role, spec in pinch.items():
+        if not isinstance(spec, dict):
+            raise ProfileError(f"pinch_anchors.{role} must be an object")
+        indices = spec.get("indices")
+        if not isinstance(indices, list) or not indices:
+            raise ProfileError(f"pinch_anchors.{role}.indices must be non-empty")
+        _finite_vector(spec.get("center"), length=len(indices), field=f"{role}.center")
+        scale = _finite_vector(
+            spec.get("scale"), length=len(indices), field=f"{role}.scale"
+        )
+        if any(item <= 0.0 for item in scale):
+            raise ProfileError(f"pinch_anchors.{role}.scale must be positive")
+        if _finite_scalar(
+            spec.get("open_distance", 0.0),
+            field=f"pinch_anchors.{role}.open_distance",
+        ) <= 0.0:
+            raise ProfileError(f"pinch_anchors.{role}.open_distance must be positive")
+    if not isinstance(profile.get("approved_for_runtime"), bool):
+        raise ProfileError("approved_for_runtime must be boolean")
+    if profile["approved_for_runtime"]:
+        def validate_repeats(summary: Mapping[str, Any], field: str) -> None:
+            repeats = summary.get("repeats")
+            if not isinstance(repeats, list) or len(repeats) != 3:
+                raise ProfileError(
+                    f"approved glove profile requires exactly three repeats for {field}"
+                )
+            repeat_total = 0
+            for repeat_index, repeat in enumerate(repeats):
+                if not isinstance(repeat, dict):
+                    raise ProfileError(f"{field}.repeats must contain objects")
+                try:
+                    repeat_count = int(repeat.get("sample_count", 0))
+                except (TypeError, ValueError) as exc:
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] count is invalid"
+                    ) from exc
+                if repeat_count < 50:
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] requires 50 frames"
+                    )
+                repeat_total += repeat_count
+                repeat_frames = repeat.get("raw_frames")
+                if (
+                    not isinstance(repeat_frames, list)
+                    or len(repeat_frames) != repeat_count
+                ):
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] frames are inconsistent"
+                    )
+                for frame_index, frame in enumerate(repeat_frames):
+                    _finite_vector(
+                        frame,
+                        length=21,
+                        field=(
+                            f"{field}.repeats[{repeat_index}]"
+                            f".raw_frames[{frame_index}]"
+                        ),
+                    )
+                _finite_vector(
+                    repeat.get("median"),
+                    length=21,
+                    field=f"{field}.repeats[{repeat_index}].median",
+                )
+                _finite_vector(
+                    repeat.get("mad"),
+                    length=21,
+                    field=f"{field}.repeats[{repeat_index}].mad",
+                )
+            if repeat_total != int(summary.get("sample_count", 0)):
+                raise ProfileError(
+                    f"{field} aggregate count does not match its repeats"
+                )
+
+        for name in GLOVE_STATIC_GESTURES:
+            validate_repeats(poses[name], f"poses.{name}")
+        dynamic = profile.get("dynamic_captures")
+        if not isinstance(dynamic, dict) or not set(
+            GLOVE_DYNAMIC_GESTURES
+        ).issubset(dynamic):
+            raise ProfileError(
+                "approved glove profile requires every dynamic sweep"
+            )
+        for name in GLOVE_DYNAMIC_GESTURES:
+            validate_repeats(
+                dynamic[name],
+                f"dynamic_captures.{name}",
+            )
+    return profile
+
+
+def load_robot_profile(
+    path: str | Path, expected_model: str | None = None
+) -> dict[str, Any]:
+    """Load and validate a G20 or O6 single-device profile."""
+
+    profile = _load_json(path)
+    if profile.get("schema_version") != SCHEMA_VERSION:
+        raise ProfileError("unsupported robot profile schema_version")
+    if profile.get("profile_kind", profile.get("kind")) != "robot_hand":
+        raise ProfileError("profile_kind must be robot_hand")
+    model = str(profile.get("model", "")).upper()
+    if model not in MODEL_COMMAND_LENGTHS:
+        raise ProfileError(f"unsupported robot model: {model}")
+    if expected_model and model != expected_model.upper():
+        raise ProfileError(
+            f"profile model {model} does not match expected {expected_model.upper()}"
+        )
+    if profile.get("side") != "left":
+        raise ProfileError("robot profile must describe a left hand")
+    if profile.get("calibration") != "provisional":
+        raise ProfileError("robot profile calibration must be provisional")
+    if not isinstance(profile.get("approved_for_control"), bool):
+        raise ProfileError("approved_for_control must be boolean")
+    mapping_strategy = profile.get(
+        "mapping_strategy",
+        "paired_continuous_v1",
+    )
+    if mapping_strategy != "paired_continuous_v1":
+        raise ProfileError(
+            f"unsupported robot mapping_strategy: {mapping_strategy}"
+        )
+    profile["mapping_strategy"] = mapping_strategy
+
+    length = MODEL_COMMAND_LENGTHS[model]
+    if profile.get("command_length") != length:
+        raise ProfileError(f"{model}.command_length must be {length}")
+    names = profile.get("command_names")
+    if not isinstance(names, list) or len(names) != length or len(set(names)) != length:
+        raise ProfileError(f"{model}.command_names must contain {length} unique names")
+    bounds = profile.get("command_bounds")
+    if not isinstance(bounds, list) or len(bounds) != length:
+        raise ProfileError(f"{model}.command_bounds must contain {length} pairs")
+    for index, pair in enumerate(bounds):
+        values = _finite_vector(pair, length=2, field=f"command_bounds[{index}]")
+        if values[0] < 0 or values[1] > 255 or values[0] > values[1]:
+            raise ProfileError(f"invalid command bounds at index {index}")
+        bounds[index] = values
+
+    poses = profile.get("poses")
+    if not isinstance(poses, dict) or "open_spread" not in poses or "fist" not in poses:
+        raise ProfileError("robot profile requires open_spread and fist poses")
+    required_gestures = set(G20_GESTURES if model == "G20" else O6_GESTURES)
+    missing_poses = required_gestures - set(poses)
+    if missing_poses:
+        raise ProfileError(f"{model} profile is missing poses: {sorted(missing_poses)}")
+    for pose_name, pose in poses.items():
+        if not isinstance(pose, dict):
+            raise ProfileError(f"poses.{pose_name} must be an object")
+        pose["cmd_u8"] = _finite_vector(
+            pose.get("cmd_u8"),
+            length=length,
+            field=f"poses.{pose_name}.cmd_u8",
+            minimum=0,
+            maximum=255,
+        )
+        if any(
+            value < bounds[index][0] or value > bounds[index][1]
+            for index, value in enumerate(pose["cmd_u8"])
+        ):
+            raise ProfileError(
+                f"poses.{pose_name}.cmd_u8 exceeds command_bounds"
+            )
+        pose_command_names = pose.get("command_names")
+        if pose_command_names is not None and list(pose_command_names) != list(names):
+            raise ProfileError(
+                f"poses.{pose_name}.command_names do not match the profile"
+            )
+        state = pose.get("state_u8", [])
+        state_names = pose.get("state_names", [])
+        if state:
+            pose["state_u8"] = _finite_vector(
+                state,
+                length=length,
+                field=f"poses.{pose_name}.state_u8",
+                minimum=0,
+                maximum=255,
+            )
+            if (
+                not isinstance(state_names, list)
+                or len(state_names) != length
+                or len(set(state_names)) != length
+            ):
+                raise ProfileError(
+                    f"poses.{pose_name}.state_names are invalid"
+                )
+        status = str(pose.get("status", ""))
+        if status not in {"exact", "approximate", "unsupported"}:
+            raise ProfileError(f"poses.{pose_name}.status is invalid")
+
+    channels = profile.get("channels")
+    if not isinstance(channels, list) or not channels:
+        raise ProfileError("robot profile channels must be non-empty")
+    seen_command_indices: set[int] = set()
+    seen_channel_names: set[str] = set()
+    for channel in channels:
+        if not isinstance(channel, dict):
+            raise ProfileError("each channel must be an object")
+        name = str(channel.get("name", ""))
+        index = channel.get("index")
+        if not name or name in seen_channel_names:
+            raise ProfileError(f"duplicate or empty channel name: {name}")
+        if not isinstance(index, int) or index < 0 or index >= length:
+            raise ProfileError(f"invalid command index for channel {name}")
+        if index in seen_command_indices:
+            raise ProfileError(f"duplicate command_index {index}")
+        if str(names[index]) != name:
+            raise ProfileError(
+                f"channel {name} does not match command_names[{index}]"
+            )
+        seen_channel_names.add(name)
+        seen_command_indices.add(index)
+        sources = channel.get("sources")
+        if not isinstance(sources, dict) or not sources:
+            raise ProfileError(f"channel {name} has no intent_sources")
+        if any(source not in BASE_INTENT_NAMES for source in sources):
+            raise ProfileError(f"channel {name} references an unknown intent")
+        try:
+            weights = [float(weight) for weight in sources.values()]
+        except (TypeError, ValueError) as exc:
+            raise ProfileError(
+                f"channel {name} has non-numeric source weights"
+            ) from exc
+        if not all(math.isfinite(weight) and weight >= 0.0 for weight in weights):
+            raise ProfileError(f"channel {name} has invalid source weights")
+        if not math.isclose(sum(weights), 1.0, abs_tol=1e-6):
+            raise ProfileError(f"channel {name} source weights must sum to one")
+        channel["sources"] = {
+            str(source): float(weight)
+            for source, weight in zip(sources, weights)
+        }
+        mapping_kind = channel.get("mapping")
+        if mapping_kind not in {"monotonic_piecewise", "piecewise"}:
+            raise ProfileError(f"channel {name} has an invalid mapping kind")
+        points = channel.get("points")
+        if not isinstance(points, list) or len(points) < 2:
+            raise ProfileError(f"channel {name} requires at least two command_points")
+        normalized_points = []
+        for point in points:
+            values = _finite_vector(point, length=2, field=f"{name}.command_point")
+            if values[0] < 0.0 or values[0] > 1.0 or values[1] < 0 or values[1] > 255:
+                raise ProfileError(f"channel {name} has an invalid command point")
+            if values[1] < bounds[index][0] or values[1] > bounds[index][1]:
+                raise ProfileError(f"channel {name} command point exceeds bounds")
+            normalized_points.append(values)
+        channel["points"] = normalized_points
+        points = normalized_points
+        ordered_points = sorted((float(point[0]), float(point[1])) for point in points)
+        distinct_x = {point[0] for point in ordered_points}
+        if len(distinct_x) < 2:
+            raise ProfileError(f"channel {name} needs two distinct activations")
+        deltas = [
+            right[1] - left[1]
+            for left, right in zip(ordered_points, ordered_points[1:])
+            if right[0] > left[0]
+        ]
+        if mapping_kind == "monotonic_piecewise" and deltas and not (
+            all(delta >= -1e-9 for delta in deltas)
+            or all(delta <= 1e-9 for delta in deltas)
+        ):
+            raise ProfileError(f"channel {name} command_points are not monotonic")
+    simulation = profile.get("simulation")
+    if not isinstance(simulation, dict):
+        raise ProfileError("simulation must be an object")
+    simulation_mapping_strategy = simulation.get(
+        "mapping_strategy",
+        "semantic_urdf_v1",
+    )
+    if simulation_mapping_strategy != "semantic_urdf_v1":
+        raise ProfileError(
+            "simulation.mapping_strategy must be semantic_urdf_v1"
+        )
+    simulation["mapping_strategy"] = simulation_mapping_strategy
+    urdf_hash = str(simulation.get("urdf_sha256", ""))
+    if len(urdf_hash) != 64 or any(
+        character not in "0123456789abcdef" for character in urdf_hash.lower()
+    ):
+        raise ProfileError("simulation.urdf_sha256 must be a SHA-256 hex digest")
+    sim_names = simulation.get("joint_names")
+    if (
+        not isinstance(sim_names, list)
+        or len(sim_names) != len(channels)
+        or len(set(sim_names)) != len(sim_names)
+    ):
+        raise ProfileError("simulation.joint_names must match the active channels")
+    if list(sim_names) != [str(channel["name"]) for channel in channels]:
+        raise ProfileError("simulation.joint_names must use channel order")
+    for key in ("open", "closed", "lower", "upper"):
+        simulation[key] = _finite_vector(
+            simulation.get(key), length=len(channels), field=f"simulation.{key}"
+        )
+    if any(
+        low > high
+        for low, high in zip(simulation["lower"], simulation["upper"])
+    ):
+        raise ProfileError("simulation lower limit exceeds upper limit")
+    for endpoint in ("open", "closed"):
+        if any(
+            value < low or value > high
+            for value, low, high in zip(
+                simulation[endpoint],
+                simulation["lower"],
+                simulation["upper"],
+            )
+        ):
+            raise ProfileError(f"simulation.{endpoint} exceeds URDF limits")
+
+    reserved = profile.get("reserved_channels", {})
+    if not isinstance(reserved, dict):
+        raise ProfileError("reserved_channels must be an object")
+    for raw_index, raw_value in reserved.items():
+        try:
+            index = int(raw_index)
+        except (TypeError, ValueError) as exc:
+            raise ProfileError("reserved channel index must be an integer") from exc
+        value = _finite_scalar(raw_value, field=f"reserved_channels.{raw_index}")
+        if index < 0 or index >= length or value < 0 or value > 255:
+            raise ProfileError("reserved_channels contains an invalid entry")
+        if float(bounds[index][0]) != value or float(bounds[index][1]) != value:
+            raise ProfileError("reserved channel bounds must be fixed to its value")
+    if seen_command_indices | {int(index) for index in reserved} != set(range(length)):
+        raise ProfileError("active and reserved channels do not cover every command")
+    filters = profile.get("filter")
+    if not isinstance(filters, dict):
+        raise ProfileError("filter must be an object")
+    mode = str(filters.get("mode", "acceleration_limited")).strip().lower()
+    if mode not in {"passthrough", "ema", "acceleration_limited"}:
+        raise ProfileError(
+            "filter.mode must be passthrough, ema or acceleration_limited"
+        )
+    alpha = _finite_scalar(
+        filters.get("ema_alpha", -1.0), field="filter.ema_alpha"
+    )
+    max_step = _finite_scalar(
+        filters.get("max_step_u8", -1.0), field="filter.max_step_u8"
+    )
+    max_acceleration = _finite_scalar(
+        filters.get(
+            "max_acceleration_u8_per_frame2",
+            max_step,
+        ),
+        field="filter.max_acceleration_u8_per_frame2",
+    )
+    deadband = _finite_scalar(
+        filters.get("deadband_u8", -1.0), field="filter.deadband_u8"
+    )
+    if (
+        not 0.0 < alpha <= 1.0
+        or max_step <= 0.0
+        or not 0.0 < max_acceleration <= max_step
+        or deadband < 0.0
+    ):
+        raise ProfileError("filter parameters are invalid")
+    filters["mode"] = mode
+    filters["ema_alpha"] = alpha
+    filters["max_step_u8"] = max_step
+    filters["max_acceleration_u8_per_frame2"] = max_acceleration
+    filters["deadband_u8"] = deadband
+    if profile.get("approved_for_control", False):
+        device = profile.get("device")
+        if not isinstance(device, dict):
+            raise ProfileError("approved profile requires device metadata")
+        if not str(device.get("serial_number", "")).strip():
+            raise ProfileError("approved profile requires a serial_number")
+        if not str(device.get("can_interface", "")).strip():
+            raise ProfileError("approved profile requires a CAN interface")
+        metadata = profile.get("metadata")
+        if (
+            not isinstance(metadata, dict)
+            or not metadata.get("operator_confirmed")
+            or not str(metadata.get("operator", "")).strip()
+        ):
+            raise ProfileError(
+                "approved profile requires a named human confirmation"
+            )
+        for pose_name in required_gestures:
+            pose = poses[pose_name]
+            if not pose.get("confirmed", False):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} is not confirmed"
+                )
+            if list(pose.get("command_names", ())) != list(names):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks command names"
+                )
+            if len(pose.get("state_u8", ())) != length:
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks device state"
+                )
+            if list(pose.get("state_names", ())) != list(names):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks named device state"
+                )
+    return profile

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/quality.py

@@ -0,0 +1,475 @@
+"""Offline accuracy and smoothness checks for paired retarget profiles."""
+
+from __future__ import annotations
+
+import argparse
+import json
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .constants import PINCH_ROLES
+from .mapping import (
+    CommandFilter,
+    HandIntentExtractor,
+    RobotMapper,
+)
+from .profiles import load_glove_profile, load_robot_profile
+
+
+def _active_indices(robot_profile: Mapping[str, Any]) -> tuple[int, ...]:
+    return tuple(int(channel["index"]) for channel in robot_profile["channels"])
+
+
+def _pose_indices(
+    robot_profile: Mapping[str, Any],
+    pose_name: str,
+) -> tuple[int, ...]:
+    channels = robot_profile["channels"]
+    if pose_name.startswith("pinch_"):
+        role = pose_name.removeprefix("pinch_")
+        prefixes = ("thumb_", f"{role}_")
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith(prefixes)
+        )
+    if pose_name in {"thumb_max_out", "thumb_across"}:
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith("thumb_")
+        )
+    if pose_name == "fingers_together":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).endswith("_mcp_roll")
+        )
+    if pose_name in {"tabletop", "hook"}:
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if not str(channel["name"]).startswith("thumb_")
+            and not str(channel["name"]).endswith("_mcp_roll")
+        )
+    if pose_name == "fist":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if not str(channel["name"]).endswith("_mcp_roll")
+        )
+    return _active_indices(robot_profile)
+
+
+def _dynamic_target_indices(
+    robot_profile: Mapping[str, Any],
+    capture_name: str,
+) -> tuple[int, ...]:
+    channels = robot_profile["channels"]
+    if capture_name == "finger_splay_sweep":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).endswith("_mcp_roll")
+        )
+    if capture_name.startswith("thumb_"):
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith("thumb_")
+        )
+    role = capture_name.removesuffix("_flex_sweep")
+    return tuple(
+        int(channel["index"])
+        for channel in channels
+        if str(channel["name"]).startswith(f"{role}_")
+        and not str(channel["name"]).endswith("_mcp_roll")
+    )
+
+
+def _repeat_groups(summary: Mapping[str, Any]) -> list[Sequence[Sequence[float]]]:
+    repeats = summary.get("repeats")
+    if isinstance(repeats, list) and repeats:
+        return [
+            repeat.get("raw_frames", [])
+            for repeat in repeats
+            if isinstance(repeat, Mapping)
+        ]
+    return [summary.get("raw_frames", [])]
+
+
+def evaluate_mapping_quality(
+    glove_profile: Mapping[str, Any],
+    robot_profile: Mapping[str, Any],
+) -> dict[str, Any]:
+    """Replay profile captures without touching ROS or physical hardware."""
+
+    extractor = HandIntentExtractor(glove_profile)
+    mapper = RobotMapper(
+        robot_profile,
+        glove_profile,
+        command_filter_config={
+            "mode": "passthrough",
+            "ema_alpha": 1.0,
+            "max_step_u8": 255.0,
+            "max_acceleration_u8_per_frame2": 255.0,
+            "deadband_u8": 0.0,
+        },
+    )
+    static: dict[str, Any] = {}
+    hard_failures: list[str] = []
+    warnings: list[str] = []
+    for pose_name, glove_pose in glove_profile["poses"].items():
+        if pose_name not in robot_profile["poses"]:
+            continue
+        indices = _pose_indices(robot_profile, str(pose_name))
+        if not indices:
+            continue
+        intent = extractor.extract(glove_pose["median"])
+        command, _, gates = mapper._raw_map(intent)
+        target = np.asarray(
+            robot_profile["poses"][pose_name]["cmd_u8"],
+            dtype=float,
+        )
+        errors = np.abs(command[list(indices)] - target[list(indices)])
+        maximum = float(np.max(errors))
+        static[str(pose_name)] = {
+            "relevant_channel_count": len(indices),
+            "max_abs_error_u8": maximum,
+            "mean_abs_error_u8": float(np.mean(errors)),
+            "active_branches": {
+                str(name): float(value)
+                for name, value in gates.items()
+                if value > 0.0
+            },
+        }
+        if maximum > 5.0:
+            hard_failures.append(
+                f"{pose_name}有效通道复现误差{maximum:.1f} > 5"
+            )
+
+    pinch_confusion: dict[str, Any] = {}
+    for target_role in PINCH_ROLES:
+        pose_name = f"pinch_{target_role}"
+        intent = extractor.extract(
+            glove_profile["poses"][pose_name]["median"]
+        )
+        scores = {
+            role: float(intent[f"pinch_{role}"])
+            for role in PINCH_ROLES
+        }
+        gates = mapper._pinch_gates(intent)
+        winner = max(scores, key=scores.get)
+        pinch_confusion[target_role] = {
+            "winner": winner,
+            "scores": scores,
+            "gates": gates,
+        }
+        if winner != target_role or gates[target_role] < 0.95:
+            hard_failures.append(
+                f"{pose_name}竞争识别失败: winner={winner}, "
+                f"gate={gates[target_role]:.3f}"
+            )
+
+    active = _active_indices(robot_profile)
+    dynamic: dict[str, Any] = {}
+    all_steps: list[np.ndarray] = []
+    all_unchanged: list[np.ndarray] = []
+    all_execution_steps: list[np.ndarray] = []
+    all_execution_unchanged: list[np.ndarray] = []
+    execution_filter_config = dict(robot_profile["filter"])
+    for capture_name, summary in glove_profile.get(
+        "dynamic_captures", {}
+    ).items():
+        if not isinstance(summary, Mapping):
+            continue
+        target_indices = _dynamic_target_indices(
+            robot_profile,
+            str(capture_name),
+        )
+        off_indices = tuple(
+            index
+            for index in active
+            if index not in set(target_indices)
+        )
+        repeats: list[dict[str, float]] = []
+        for frames in _repeat_groups(summary):
+            values = np.asarray(frames, dtype=float)
+            if values.ndim != 2 or values.shape[0] < 2:
+                continue
+            commands = []
+            semantic_rows = []
+            for frame in values:
+                # Runtime defaults to the legacy left-G20 timing path: use
+                # the latest glove frame directly and let the motor firmware
+                # interpolate the calibrated target.
+                intent = extractor.extract(frame)
+                semantic_rows.append(intent)
+                commands.append(mapper._raw_map(intent)[0])
+            command_array = np.asarray(commands, dtype=float)
+            active_command = command_array[:, list(active)]
+            steps = np.abs(np.diff(active_command, axis=0))
+            unchanged = (
+                np.max(
+                    np.abs(
+                        np.diff(np.rint(active_command), axis=0)
+                    ),
+                    axis=1,
+                )
+                == 0.0
+            )
+            all_steps.append(steps.reshape(-1))
+            all_unchanged.append(unchanged)
+            execution_filter = CommandFilter(
+                command_array[0],
+                execution_filter_config,
+            )
+            execution_command = np.asarray(
+                [
+                    execution_filter.apply(command)
+                    for command in command_array
+                ],
+                dtype=float,
+            )[:, list(active)]
+            execution_steps = np.abs(
+                np.diff(execution_command, axis=0)
+            )
+            execution_unchanged = (
+                np.max(
+                    np.abs(
+                        np.diff(
+                            np.rint(execution_command),
+                            axis=0,
+                        )
+                    ),
+                    axis=1,
+                )
+                == 0.0
+            )
+            all_execution_steps.append(execution_steps.reshape(-1))
+            all_execution_unchanged.append(execution_unchanged)
+            target_span = (
+                float(
+                    np.max(
+                        np.ptp(
+                            command_array[:, list(target_indices)],
+                            axis=0,
+                        )
+                    )
+                )
+                if target_indices
+                else 0.0
+            )
+            off_span = (
+                float(
+                    np.max(
+                        np.ptp(
+                            command_array[:, list(off_indices)],
+                            axis=0,
+                        )
+                    )
+                )
+                if off_indices
+                else 0.0
+            )
+            repeats.append(
+                {
+                    "target_span_u8": target_span,
+                    "off_target_span_u8": off_span,
+                    "step_p95_u8": float(np.percentile(steps, 95)),
+                    "step_max_u8": float(np.max(steps)),
+                    "integer_unchanged_fraction": float(
+                        np.mean(unchanged)
+                    ),
+                    "execution_step_p95_u8": float(
+                        np.percentile(execution_steps, 95)
+                    ),
+                    "execution_step_max_u8": float(
+                        np.max(execution_steps)
+                    ),
+                    "execution_integer_unchanged_fraction": float(
+                        np.mean(execution_unchanged)
+                    ),
+                }
+            )
+        dynamic[str(capture_name)] = {"repeats": repeats}
+        if str(capture_name).endswith("_flex_sweep") and repeats:
+            median_target = float(
+                np.median([item["target_span_u8"] for item in repeats])
+            )
+            median_off = float(
+                np.median(
+                    [item["off_target_span_u8"] for item in repeats]
+                )
+            )
+            if median_off > max(15.0, 0.2 * median_target):
+                warnings.append(
+                    f"{capture_name}非目标通道跨度{median_off:.1f}偏大"
+                )
+        if capture_name == "finger_splay_sweep":
+            flex_names = (
+                "index_root",
+                "index_tip",
+                "middle_root",
+                "middle_tip",
+                "ring_root",
+                "ring_tip",
+                "pinky_root",
+                "pinky_tip",
+            )
+            flex_ranges = []
+            for frames in _repeat_groups(summary):
+                rows = [
+                    extractor.extract(frame)
+                    for frame in frames
+                ]
+                if rows:
+                    flex_ranges.append(
+                        max(
+                            float(
+                                np.ptp(
+                                    [row[name] for row in rows]
+                                )
+                            )
+                            for name in flex_names
+                        )
+                    )
+            if flex_ranges and float(np.median(flex_ranges)) > 0.5:
+                warnings.append(
+                    "finger_splay_sweep中检测到明显屈伸，建议重采四指开合轨迹"
+                )
+
+    flattened_steps = (
+        np.concatenate(all_steps) if all_steps else np.asarray([], dtype=float)
+    )
+    flattened_unchanged = (
+        np.concatenate(all_unchanged)
+        if all_unchanged
+        else np.asarray([], dtype=bool)
+    )
+    flattened_execution_steps = (
+        np.concatenate(all_execution_steps)
+        if all_execution_steps
+        else np.asarray([], dtype=float)
+    )
+    flattened_execution_unchanged = (
+        np.concatenate(all_execution_unchanged)
+        if all_execution_unchanged
+        else np.asarray([], dtype=bool)
+    )
+    return {
+        "kind": "linkerforce_v2_mapping_quality",
+        "mapping_mode": mapper.mapping_mode,
+        "model": str(robot_profile["model"]).upper(),
+        "static": static,
+        "pinch_confusion": pinch_confusion,
+        "dynamic": dynamic,
+        "summary": {
+            "static_max_abs_error_u8": max(
+                (
+                    float(item["max_abs_error_u8"])
+                    for item in static.values()
+                ),
+                default=0.0,
+            ),
+            "dynamic_step_p95_u8": (
+                float(np.percentile(flattened_steps, 95))
+                if flattened_steps.size
+                else None
+            ),
+            "dynamic_step_max_u8": (
+                float(np.max(flattened_steps))
+                if flattened_steps.size
+                else None
+            ),
+            "integer_unchanged_fraction": (
+                float(np.mean(flattened_unchanged))
+                if flattened_unchanged.size
+                else None
+            ),
+            "execution_filter": {
+                "mode": str(execution_filter_config["mode"]),
+                "ema_alpha": float(
+                    execution_filter_config["ema_alpha"]
+                ),
+                "max_step_u8": float(
+                    execution_filter_config["max_step_u8"]
+                ),
+                "deadband_u8": float(
+                    execution_filter_config["deadband_u8"]
+                ),
+            },
+            "execution_step_p95_u8": (
+                float(np.percentile(flattened_execution_steps, 95))
+                if flattened_execution_steps.size
+                else None
+            ),
+            "execution_step_max_u8": (
+                float(np.max(flattened_execution_steps))
+                if flattened_execution_steps.size
+                else None
+            ),
+            "execution_integer_unchanged_fraction": (
+                float(np.mean(flattened_execution_unchanged))
+                if flattened_execution_unchanged.size
+                else None
+            ),
+            "splay_compensation": {
+                name: list(coefficients)
+                for name, coefficients
+                in extractor.splay_compensation.items()
+            },
+            "hard_failures": hard_failures,
+            "warnings": warnings,
+            "passed": not hard_failures,
+        },
+    }
+
+
+def main(argv: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(
+        description="离线检查FFG与机械手profile的准确性、串指和目标连续性"
+    )
+    parser.add_argument("--glove", required=True, help="FFG profile JSON")
+    parser.add_argument("--robot", required=True, help="G20/O6 profile JSON")
+    parser.add_argument("--model", choices=("G20", "O6"), required=True)
+    parser.add_argument("--output", help="可选的完整JSON报告路径")
+    parser.add_argument(
+        "--strict",
+        action="store_true",
+        help="硬错误存在时以退出码2结束",
+    )
+    args = parser.parse_args(argv)
+    glove = load_glove_profile(args.glove)
+    robot = load_robot_profile(args.robot, args.model)
+    report = evaluate_mapping_quality(glove, robot)
+    if args.output:
+        output = Path(args.output).expanduser().resolve()
+        output.parent.mkdir(parents=True, exist_ok=True)
+        temporary = output.with_suffix(output.suffix + ".tmp")
+        temporary.write_text(
+            json.dumps(report, ensure_ascii=False, indent=2, sort_keys=True)
+            + "\n",
+            encoding="utf-8",
+        )
+        temporary.replace(output)
+    summary = report["summary"]
+    print(
+        json.dumps(
+            {
+                "mapping_mode": report["mapping_mode"],
+                **summary,
+            },
+            ensure_ascii=False,
+            indent=2,
+            sort_keys=True,
+        )
+    )
+    if args.strict and not summary["passed"]:
+        raise SystemExit(2)
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/safety.py

@@ -0,0 +1,33 @@
+"""Pure safety decisions shared by runtime and tests."""
+
+from __future__ import annotations
+
+from typing import Mapping
+
+
+def evaluate_timeouts(
+    *,
+    now: float,
+    enabled: Mapping[str, bool],
+    last_glove_time: float,
+    driver_state_time: Mapping[str, float],
+    driver_state_valid: Mapping[str, bool],
+    glove_timeout: float,
+    driver_timeout: float,
+) -> tuple[bool, tuple[str, ...]]:
+    """Return (disable_all, individually_timed_out_models)."""
+
+    if any(enabled.values()) and (
+        last_glove_time <= 0.0 or now - last_glove_time > glove_timeout
+    ):
+        return True, ()
+    timed_out = tuple(
+        model
+        for model, is_enabled in enabled.items()
+        if is_enabled
+        and (
+            not bool(driver_state_valid.get(model, False))
+            or now - float(driver_state_time.get(model, 0.0)) > driver_timeout
+        )
+    )
+    return False, timed_out

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/session_manifest.py

@@ -0,0 +1,181 @@
+"""Create a traceable manifest for provisional rosbag2 data collection."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import hashlib
+import json
+from pathlib import Path
+import shlex
+from typing import Sequence
+
+from .profiles import load_glove_profile, load_robot_profile, save_profile
+
+
+TOPICS = (
+    "/ffg/left/raw_joint_state",
+    "/retarget/left/hand_intent",
+    "/retarget/left/frame_meta",
+    "/retarget/g20/left/actuation_target",
+    "/retarget/o6/left/actuation_target",
+    "/retarget/g20/left/joint_target_nominal",
+    "/retarget/o6/left/joint_target_nominal",
+    "/retarget/g20/left/cmd_u8_preview",
+    "/retarget/o6/left/cmd_u8_preview",
+    "/g20/cb_left_hand_control_cmd",
+    "/o6/cb_left_hand_control_cmd",
+    "/g20/cb_left_hand_state",
+    "/o6/cb_left_hand_state",
+    "/sim/mujoco/g20/left/joint_state",
+    "/sim/mujoco/o6/left/joint_state",
+    "/sim/isaac/g20/left/joint_state",
+    "/sim/isaac/o6/left/joint_state",
+    "/ffg_dual_retarget/status",
+)
+
+
+def _sha256(path: str) -> str:
+    source = Path(path).expanduser().resolve()
+    digest = hashlib.sha256()
+    with source.open("rb") as handle:
+        for block in iter(lambda: handle.read(1024 * 1024), b""):
+            digest.update(block)
+    return digest.hexdigest()
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="生成provisional数采manifest")
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--session-id", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--glove-profile", required=True)
+    parser.add_argument("--wear-check", required=True)
+    parser.add_argument("--wear-check-max-age-hours", type=float, default=12.0)
+    parser.add_argument("--g20-profile", required=True)
+    parser.add_argument("--o6-profile", required=True)
+    parser.add_argument("--g20-urdf", required=True)
+    parser.add_argument("--o6-urdf", required=True)
+    parser.add_argument("--g20-firmware", default="unknown")
+    parser.add_argument("--o6-firmware", default="unknown")
+    parser.add_argument("--g20-serial-number", required=True)
+    parser.add_argument("--o6-serial-number", required=True)
+    parser.add_argument("--bag-output")
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args = _parser().parse_args(argv)
+    bag_output = args.bag_output or f"bags/{args.session_id}"
+    glove_profile = load_glove_profile(args.glove_profile)
+    g20_profile = load_robot_profile(args.g20_profile, "G20")
+    o6_profile = load_robot_profile(args.o6_profile, "O6")
+    if not glove_profile["approved_for_runtime"]:
+        raise SystemExit("FFG profile不完整，禁止生成数采session")
+    if not g20_profile["approved_for_control"] or not o6_profile[
+        "approved_for_control"
+    ]:
+        raise SystemExit("G20/O6 profile未全部获批，禁止生成数采session")
+    wear_path = Path(args.wear_check).expanduser().resolve()
+    wear_check = json.loads(wear_path.read_text(encoding="utf-8"))
+    if (
+        wear_check.get("kind") != "ffg_wear_check"
+        or not wear_check.get("passed")
+        or wear_check.get("glove_profile_sha256")
+        != glove_profile["_profile_sha256"]
+    ):
+        raise SystemExit("快速佩戴检查失败或与FFG profile不匹配")
+    checked_at = datetime.fromisoformat(str(wear_check["checked_at"]))
+    if checked_at.tzinfo is None:
+        checked_at = checked_at.replace(tzinfo=timezone.utc)
+    wear_age_hours = (
+        datetime.now(timezone.utc) - checked_at.astimezone(timezone.utc)
+    ).total_seconds() / 3600.0
+    if (
+        wear_age_hours < 0.0
+        or wear_age_hours > args.wear_check_max_age_hours
+    ):
+        raise SystemExit("快速佩戴检查已过期")
+    expected_devices = {
+        "g20": (g20_profile, args.g20_serial_number, "can0"),
+        "o6": (o6_profile, args.o6_serial_number, "can1"),
+    }
+    for model, (profile, serial_number, can_interface) in expected_devices.items():
+        device = profile["device"]
+        if (
+            device["serial_number"] != serial_number
+            or device["can_interface"] != can_interface
+        ):
+            raise SystemExit(f"{model.upper()} profile的SN/CAN与session不匹配")
+        requested_firmware = getattr(args, f"{model}_firmware")
+        if (
+            requested_firmware != "unknown"
+            and requested_firmware != device["firmware"]
+        ):
+            raise SystemExit(f"{model.upper()}固件版本与profile不匹配")
+    urdf_paths = {"g20": args.g20_urdf, "o6": args.o6_urdf}
+    for model, path in urdf_paths.items():
+        if _sha256(path) != expected_devices[model][0]["simulation"]["urdf_sha256"]:
+            raise SystemExit(f"{model.upper()} URDF哈希与profile不匹配")
+    manifest = {
+        "schema_version": 1,
+        "session_id": args.session_id,
+        "created_at": datetime.now(timezone.utc).isoformat(),
+        "operator": args.operator,
+        "calibration": "provisional",
+        "q_gt": None,
+        "angle_accuracy_claim": False,
+        "profiles": {
+            "glove": {
+                "path": str(Path(args.glove_profile).expanduser().resolve()),
+                "sha256": glove_profile["_profile_sha256"],
+            },
+            "wear_check": {
+                "path": str(wear_path),
+                "sha256": _sha256(args.wear_check),
+                "checked_at": wear_check["checked_at"],
+            },
+            "g20": {
+                "path": str(Path(args.g20_profile).expanduser().resolve()),
+                "sha256": g20_profile["_profile_sha256"],
+            },
+            "o6": {
+                "path": str(Path(args.o6_profile).expanduser().resolve()),
+                "sha256": o6_profile["_profile_sha256"],
+            },
+        },
+        "urdf": {
+            "g20": {
+                "path": str(Path(args.g20_urdf).expanduser().resolve()),
+                "sha256": _sha256(args.g20_urdf),
+            },
+            "o6": {
+                "path": str(Path(args.o6_urdf).expanduser().resolve()),
+                "sha256": _sha256(args.o6_urdf),
+            },
+        },
+        "devices": {
+            "g20": {
+                "serial_number": args.g20_serial_number,
+                "firmware": g20_profile["device"]["firmware"],
+                "can_interface": "can0",
+            },
+            "o6": {
+                "serial_number": args.o6_serial_number,
+                "firmware": o6_profile["device"]["firmware"],
+                "can_interface": "can1",
+            },
+        },
+        "topics": list(TOPICS),
+        "bag_output": bag_output,
+    }
+    output = save_profile(manifest, args.output)
+    command = ["ros2", "bag", "record", "-o", bag_output, *TOPICS]
+    print(f"manifest已保存: {output}")
+    print("录制命令：")
+    print(shlex.join(command))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/simulation.py

@@ -0,0 +1,60 @@
+"""Dependency-free joint-name contract used by MuJoCo and Isaac bridges."""
+
+from __future__ import annotations
+
+from typing import Mapping, Sequence
+
+import numpy as np
+
+
+class JointNameMismatch(ValueError):
+    """Raised before a simulator receives an ambiguously ordered target."""
+
+
+def build_name_mapping(
+    incoming_names: Sequence[str], simulator_joint_names: Sequence[str]
+) -> tuple[int, ...]:
+    """Return indices that reorder a named ROS target into simulator order."""
+
+    incoming = [str(name) for name in incoming_names]
+    expected = [str(name) for name in simulator_joint_names]
+    if not incoming or len(set(incoming)) != len(incoming):
+        raise JointNameMismatch("incoming target has empty or duplicate joint names")
+    if not expected or len(set(expected)) != len(expected):
+        raise JointNameMismatch("simulator model has empty or duplicate joint names")
+    incoming_set = set(incoming)
+    expected_set = set(expected)
+    missing = sorted(expected_set - incoming_set)
+    extra = sorted(incoming_set - expected_set)
+    if missing or extra:
+        raise JointNameMismatch(
+            f"joint-name mismatch: missing={missing}, extra={extra}"
+        )
+    lookup = {name: index for index, name in enumerate(incoming)}
+    return tuple(lookup[name] for name in expected)
+
+
+def reorder_named_target(
+    incoming_names: Sequence[str],
+    incoming_positions: Sequence[float],
+    simulator_joint_names: Sequence[str],
+    limits: Mapping[str, tuple[float, float]] | None = None,
+) -> np.ndarray:
+    """Validate, reorder and limit q_nominal before simulator position control."""
+
+    if len(incoming_names) != len(incoming_positions):
+        raise JointNameMismatch("target name/position lengths differ")
+    values = np.asarray(incoming_positions, dtype=float)
+    if not np.all(np.isfinite(values)):
+        raise JointNameMismatch("target contains a non-finite position")
+    mapping = build_name_mapping(incoming_names, simulator_joint_names)
+    ordered = values[list(mapping)].copy()
+    if limits:
+        for index, name in enumerate(simulator_joint_names):
+            if name not in limits:
+                raise JointNameMismatch(f"missing simulator limit for joint {name}")
+            lower, upper = limits[name]
+            if lower > upper:
+                raise JointNameMismatch(f"invalid simulator limit for joint {name}")
+            ordered[index] = np.clip(ordered[index], lower, upper)
+    return ordered

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/verify_robot_profile.py

@@ -0,0 +1,518 @@
+"""Safely replay and manually verify calibrated robot-hand profile poses."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import math
+from pathlib import Path
+import threading
+import time
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import JointState
+
+from .calibrate_robot import POSE_LABELS
+from .constants import MODEL_COMMAND_LENGTHS
+from .profiles import load_robot_profile, save_profile
+
+
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+POSE_EXPECTATIONS = {
+    "open_spread": "五指伸直并自然分开；拇指处于自然张开位置",
+    "fingers_together": "四指伸直并拢；该姿势不评价拇指",
+    "tabletop": "四指根部弯曲、末端伸直；该姿势不评价拇指",
+    "hook": "四指根部基本伸直、末端弯曲；该姿势不评价拇指",
+    "fist": "四指根部和末端闭合，拇指自然横跨",
+    "thumb_max_out": "拇指最大外展，四指保持伸直",
+    "thumb_across": "拇指横跨掌心，四指保持伸直",
+    "pinch_index": "拇指只与食指捏合，其他三指不串动",
+    "pinch_middle": "拇指只与中指捏合，其他三指不串动",
+    "pinch_ring": "拇指只与无名指捏合，其他三指不串动",
+    "pinch_pinky": "拇指只与小指捏合，其他三指不串动",
+}
+
+
+def transition_commands(
+    start: Sequence[float],
+    target: Sequence[float],
+    *,
+    max_step_u8: float,
+) -> list[list[float]]:
+    """Return a linear transition whose per-channel step is bounded."""
+
+    start_values = np.asarray(start, dtype=float)
+    target_values = np.asarray(target, dtype=float)
+    if (
+        start_values.ndim != 1
+        or target_values.shape != start_values.shape
+        or not np.all(np.isfinite(start_values))
+        or not np.all(np.isfinite(target_values))
+    ):
+        raise ValueError("start and target must be equal-length finite vectors")
+    if not math.isfinite(max_step_u8) or max_step_u8 <= 0.0:
+        raise ValueError("max_step_u8 must be positive")
+    maximum_delta = float(np.max(np.abs(target_values - start_values)))
+    steps = max(1, int(math.ceil(maximum_delta / max_step_u8)))
+    return [
+        (
+            start_values
+            + (target_values - start_values) * (step / steps)
+        ).astype(float).tolist()
+        for step in range(1, steps + 1)
+    ]
+
+
+def state_error_summary(
+    command_names: Sequence[str],
+    target: Sequence[float],
+    state: Sequence[float],
+    *,
+    active_indices: Sequence[int],
+) -> dict[str, Any]:
+    """Summarize command/state disagreement without treating it as angle GT."""
+
+    target_values = np.asarray(target, dtype=float)
+    state_values = np.asarray(state, dtype=float)
+    indices = np.asarray(active_indices, dtype=int)
+    if (
+        target_values.shape != state_values.shape
+        or target_values.shape != (len(command_names),)
+        or indices.ndim != 1
+        or len(indices) < 1
+    ):
+        raise ValueError("invalid target/state vectors")
+    error = np.abs(target_values - state_values)
+    worst_indices = sorted(
+        (int(index) for index in indices),
+        key=lambda index: (-float(error[index]), str(command_names[index])),
+    )[:5]
+    return {
+        "mean_abs_error_u8": float(np.mean(error[indices])),
+        "max_abs_error_u8": float(np.max(error[indices])),
+        "worst_channels": [
+            {
+                "name": str(command_names[index]),
+                "target_u8": float(target_values[index]),
+                "state_u8": float(state_values[index]),
+                "abs_error_u8": float(error[index]),
+            }
+            for index in worst_indices
+        ],
+    }
+
+
+def verification_document(
+    profile: Mapping[str, Any],
+    *,
+    operator: str,
+    command_topic: str,
+    state_topic: str,
+    results: Mapping[str, Mapping[str, Any]],
+) -> dict[str, Any]:
+    """Build a traceable report separate from the calibration profile."""
+
+    return {
+        "schema_version": 1,
+        "kind": "robot_pose_verification",
+        "updated_at": datetime.now(timezone.utc).isoformat(),
+        "profile_path": str(profile["_profile_path"]),
+        "profile_sha256": str(profile["_profile_sha256"]),
+        "model": str(profile["model"]),
+        "side": str(profile["side"]),
+        "device": dict(profile.get("device", {})),
+        "operator": str(operator),
+        "command_topic": str(command_topic),
+        "state_topic": str(state_topic),
+        "state_is_angle_ground_truth": False,
+        "results": dict(results),
+    }
+
+
+class HandPoseVerifyNode(Node):
+    """Publish bounded pose transitions and receive named device state."""
+
+    def __init__(
+        self,
+        *,
+        model: str,
+        command_topic: str,
+        state_topic: str,
+        command_names: Sequence[str],
+    ) -> None:
+        super().__init__(f"{model.lower()}_pose_verify")
+        self.model = model
+        self.command_topic = command_topic
+        self.state_topic = state_topic
+        self.command_names = tuple(str(name) for name in command_names)
+        self.length = MODEL_COMMAND_LENGTHS[model]
+        self.latest_state: list[float] | None = None
+        self.latest_state_time = 0.0
+        self.command_publisher: Any | None = None
+        self.state_subscription = self.create_subscription(
+            JointState,
+            state_topic,
+            self._state_callback,
+            20,
+        )
+
+    def _state_callback(self, message: JointState) -> None:
+        names = [str(name) for name in message.name]
+        values = [float(value) for value in message.position]
+        if (
+            len(names) != self.length
+            or len(set(names)) != self.length
+            or set(names) != set(self.command_names)
+            or len(values) != self.length
+            or not np.all(np.isfinite(values))
+            or any(value < 0.0 or value > 255.0 for value in values)
+        ):
+            return
+        lookup = dict(zip(names, values))
+        self.latest_state = [lookup[name] for name in self.command_names]
+        self.latest_state_time = time.monotonic()
+
+    def competing_publishers(self) -> tuple[str, ...]:
+        competitors = set()
+        for info in self.get_publishers_info_by_topic(self.command_topic):
+            if info.node_name == self.get_name():
+                continue
+            namespace = str(info.node_namespace).rstrip("/")
+            competitors.add(f"{namespace}/{info.node_name}" or info.node_name)
+        return tuple(sorted(competitors))
+
+    def enable_command_publisher(self) -> None:
+        if self.command_publisher is None:
+            self.command_publisher = self.create_publisher(
+                JointState,
+                self.command_topic,
+                COMMAND_QOS,
+            )
+
+    def publish_command(self, values: Sequence[float]) -> None:
+        if self.command_publisher is None:
+            raise RuntimeError("command publisher is not enabled")
+        message = JointState()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.name = list(self.command_names)
+        message.position = [float(value) for value in values]
+        self.command_publisher.publish(message)
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(
+        description="低速平滑回放G20/O6 profile姿势并记录人工复核结果"
+    )
+    parser.add_argument("--profile", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--topic-prefix")
+    parser.add_argument("--command-topic")
+    parser.add_argument("--state-topic")
+    parser.add_argument("--pose", help="只复核一个姿势；默认显示交互菜单")
+    parser.add_argument("--report")
+    parser.add_argument("--rate", type=float, default=30.0)
+    parser.add_argument("--max-step-u8", type=float, default=4.0)
+    parser.add_argument("--state-timeout", type=float, default=3.0)
+    parser.add_argument("--settle-seconds", type=float, default=0.7)
+    return parser
+
+
+def _report_path(profile_path: str, configured: str | None) -> Path:
+    if configured:
+        return Path(configured).expanduser().resolve()
+    path = Path(profile_path).expanduser().resolve()
+    return path.with_suffix(".verification.json")
+
+
+def _load_existing_results(
+    report_path: Path,
+    profile: Mapping[str, Any],
+) -> dict[str, dict[str, Any]]:
+    if not report_path.is_file():
+        return {}
+    try:
+        import json
+
+        report = json.loads(report_path.read_text(encoding="utf-8"))
+    except (OSError, ValueError):
+        return {}
+    if (
+        report.get("kind") != "robot_pose_verification"
+        or report.get("profile_sha256") != profile["_profile_sha256"]
+        or not isinstance(report.get("results"), dict)
+    ):
+        return {}
+    return {
+        str(name): dict(result)
+        for name, result in report["results"].items()
+        if isinstance(result, dict)
+    }
+
+
+def _wait_for_fresh_state(
+    node: HandPoseVerifyNode,
+    *,
+    newer_than: float,
+    timeout: float,
+) -> list[float]:
+    deadline = time.monotonic() + timeout
+    while rclpy.ok() and time.monotonic() < deadline:
+        if (
+            node.latest_state is not None
+            and node.latest_state_time > newer_than
+        ):
+            return list(node.latest_state)
+        time.sleep(0.02)
+    raise RuntimeError(
+        f"未在{timeout:g}秒内从{node.state_topic}收到新的有效状态"
+    )
+
+
+def _print_menu(
+    pose_names: Sequence[str],
+    results: Mapping[str, Mapping[str, Any]],
+) -> None:
+    print("\n可复核姿势：")
+    for index, pose_name in enumerate(pose_names, start=1):
+        result = results.get(pose_name)
+        if result is None:
+            marker = "未检查"
+        else:
+            marker = "通过" if result.get("visual_passed") else "未通过"
+        print(
+            f"  {index:2d}. {POSE_LABELS.get(pose_name, pose_name):12s}"
+            f" [{marker}]"
+        )
+    print("输入编号或姿势英文名；输入q退出。")
+
+
+def _select_pose(
+    entered: str,
+    pose_names: Sequence[str],
+) -> str | None:
+    value = entered.strip()
+    if not value:
+        return None
+    if value.isdigit():
+        index = int(value)
+        if 1 <= index <= len(pose_names):
+            return pose_names[index - 1]
+        return None
+    return value if value in pose_names else None
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    profile = load_robot_profile(args.profile)
+    model = str(profile["model"]).upper()
+    if not profile.get("approved_for_control", False):
+        raise SystemExit("profile未获实机控制批准，拒绝带电回放")
+    if args.rate <= 0.0 or args.rate > 60.0:
+        raise SystemExit("--rate必须在(0, 60]范围")
+    if args.max_step_u8 <= 0.0 or args.max_step_u8 > 8.0:
+        raise SystemExit("--max-step-u8必须在(0, 8]范围")
+    if args.state_timeout <= 0.0 or args.settle_seconds < 0.0:
+        raise SystemExit("状态超时必须为正数，稳定等待时间不能为负数")
+
+    lower = model.lower()
+    prefix = (args.topic_prefix or f"/{lower}").strip().rstrip("/")
+    if not prefix.startswith("/"):
+        prefix = "/" + prefix
+    command_topic = (
+        args.command_topic or f"{prefix}/cb_left_hand_control_cmd"
+    )
+    state_topic = args.state_topic or f"{prefix}/cb_left_hand_state"
+    report_path = _report_path(args.profile, args.report)
+    pose_names = [
+        name for name in POSE_LABELS if name in profile["poses"]
+    ]
+    if args.pose and args.pose not in pose_names:
+        raise SystemExit(
+            f"未知姿势{args.pose}；可选: {', '.join(pose_names)}"
+        )
+    active_indices = sorted(
+        int(channel["index"]) for channel in profile["channels"]
+    )
+    results = _load_existing_results(report_path, profile)
+
+    rclpy.init(args=ros_args)
+    node = HandPoseVerifyNode(
+        model=model,
+        command_topic=command_topic,
+        state_topic=state_topic,
+        command_names=profile["command_names"],
+    )
+    executor = SingleThreadedExecutor()
+    executor.add_node(node)
+    spin_thread = threading.Thread(target=executor.spin, daemon=True)
+    spin_thread.start()
+    try:
+        print(
+            f"{model} profile姿势复核\n"
+            f"profile: {profile['_profile_path']}\n"
+            f"sha256: {profile['_profile_sha256']}\n"
+            f"设备: {profile.get('device', {})}\n"
+            f"命令: {command_topic}\n状态: {state_topic}\n"
+            f"报告: {report_path}\n\n"
+            "安全要求：遥操节点和GUI必须停止或保持未启动；"
+            "驱动应使用低速、低力矩；带电时禁止手掰。"
+        )
+        time.sleep(0.7)
+        competitors = node.competing_publishers()
+        if competitors:
+            raise RuntimeError(
+                f"{command_topic}已有其他发布者: {', '.join(competitors)}；"
+                "请停止遥操和GUI后重试"
+            )
+        confirmation = input(
+            "确认周围安全、急停可用，并已设置低速低力矩。输入VERIFY继续："
+        ).strip()
+        if confirmation != "VERIFY":
+            print("未确认安全条件，已退出且未发送命令。")
+            return 1
+
+        node.enable_command_publisher()
+        _wait_for_fresh_state(
+            node,
+            newer_than=0.0,
+            timeout=args.state_timeout,
+        )
+        one_shot_pose = args.pose
+        while rclpy.ok():
+            if one_shot_pose:
+                pose_name = one_shot_pose
+            else:
+                _print_menu(pose_names, results)
+                entered = input("选择：").strip()
+                if entered.lower() == "q":
+                    break
+                pose_name = _select_pose(entered, pose_names)
+                if pose_name is None:
+                    print("无效选择。")
+                    continue
+
+            pose = profile["poses"][pose_name]
+            target = [float(value) for value in pose["cmd_u8"]]
+            print(
+                f"\n准备复核“{POSE_LABELS.get(pose_name, pose_name)}”"
+                f"（标定状态={pose['status']}）\n"
+                f"期望：{POSE_EXPECTATIONS.get(pose_name, '按标定定义目视检查')}"
+            )
+            move = input("输入MOVE开始平滑运动；输入其他内容取消：").strip()
+            if move != "MOVE":
+                print("已取消，未发送该姿势。")
+                if one_shot_pose:
+                    break
+                continue
+
+            competitors = node.competing_publishers()
+            if competitors:
+                raise RuntimeError(
+                    f"检测到新的命令发布者: {', '.join(competitors)}；立即停止复核"
+                )
+            state_request_time = time.monotonic()
+            start = _wait_for_fresh_state(
+                node,
+                newer_than=state_request_time,
+                timeout=args.state_timeout,
+            )
+            commands = transition_commands(
+                start,
+                target,
+                max_step_u8=args.max_step_u8,
+            )
+            print(
+                f"从当前状态分{len(commands)}步运动，"
+                f"预计{len(commands) / args.rate:.2f}秒……"
+            )
+            for index, command in enumerate(commands):
+                if index % 10 == 0:
+                    competitors = node.competing_publishers()
+                    if competitors:
+                        raise RuntimeError(
+                            "运动过程中检测到其他命令发布者，已停止发送"
+                        )
+                node.publish_command(command)
+                time.sleep(1.0 / args.rate)
+
+            last_publish_time = time.monotonic()
+            node.publish_command(target)
+            time.sleep(args.settle_seconds)
+            state = _wait_for_fresh_state(
+                node,
+                newer_than=last_publish_time,
+                timeout=args.state_timeout,
+            )
+            summary = state_error_summary(
+                profile["command_names"],
+                target,
+                state,
+                active_indices=active_indices,
+            )
+            print(
+                "状态回读（仅电机状态，不是真实角度GT）："
+                f"平均误差={summary['mean_abs_error_u8']:.2f}，"
+                f"最大误差={summary['max_abs_error_u8']:.2f}"
+            )
+            for item in summary["worst_channels"]:
+                print(
+                    f"  {item['name']}: 目标{item['target_u8']:.1f}，"
+                    f"状态{item['state_u8']:.1f}，"
+                    f"差{item['abs_error_u8']:.1f}"
+                )
+
+            while True:
+                verdict = input(
+                    "目视结果：p=通过，f=未通过，r=重放，s=跳过："
+                ).strip().lower()
+                if verdict in {"p", "f", "r", "s"}:
+                    break
+                print("请输入p/f/r/s。")
+            if verdict == "r":
+                continue
+            if verdict != "s":
+                note = input("备注（可直接回车）：").strip()
+                results[pose_name] = {
+                    "pose_label": POSE_LABELS.get(pose_name, pose_name),
+                    "profile_status": pose["status"],
+                    "verified_at": datetime.now(timezone.utc).isoformat(),
+                    "visual_passed": verdict == "p",
+                    "operator_note": note,
+                    "target_cmd_u8": target,
+                    "observed_state_u8": state,
+                    **summary,
+                }
+                save_profile(
+                    verification_document(
+                        profile,
+                        operator=args.operator,
+                        command_topic=command_topic,
+                        state_topic=state_topic,
+                        results=results,
+                    ),
+                    report_path,
+                )
+                print(f"复核结果已保存：{report_path}")
+            if one_shot_pose:
+                break
+        return 0
+    finally:
+        executor.shutdown()
+        spin_thread.join(timeout=1.0)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/package.xml

@@ -12,6 +12,12 @@
   <test_depend>python3-pytest</test_depend>
   
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
   <export>
     <build_type>ament_python</build_type>
   </export>

src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md

@@ -0,0 +1,374 @@
+# linkerforce_v2：FFG左手套同时控制G20和O6
+
+该链路与旧 `handretarget` 并存。它只使用一只左手FFG，不创建右手套读取器；运行时链路为：
+
+```text
+FFG 21维原始弧度
+  → hand_intent（模型无关、0～1）
+  → G20/O6 actuation_target（按名称）
+       ├─→ 解剖通道曲线＋局部捏合/握持校正 → cmd_u8（实机）
+       └─→ 独立语义—URDF映射 → q_nominal（仿真）
+```
+
+种子profile只给标定GUI提供安全的滑块初值，均为
+`approved_for_control=false`。它们不能使能实机，也不会成为运行时静默回退。
+
+### 分解式连续映射原则
+
+运行时不会把整只手吸附或锁定到“最相似手势”。新版将普通关节曲线、握持协调和
+捏合修正拆开：
+
+- 每根普通手指的根部、末端和侧摆只读取该手指的解剖语义，拇指运动不会再改变
+  中指、无名指或小指命令；
+- 四指屈伸使用张手、桌面、钩拳和握拳构成的标定面连续解耦根部与末端传感器串扰；
+  这些锚点定义语义坐标，不会在命令曲线中形成“先不动、随后突变”的平台区；
+- 张手、并拢、桌面、钩拳和握拳只约束其实际定义的通道，GUI中非目标滑块的残留值
+  不再污染映射；
+- 四种捏合先按当前手套profile做竞争判别，只有证据充分且领先其他候选的分支生效；
+- 捏合分支只连续修正拇指和目标手指，竞争不明确时平滑退回普通逐关节映射；
+- 握拳只对拇指增加局部协调，且必须同时满足四指握持和拇指折叠，普通拇指动作不会
+  被强制变成握拳模板；
+- 独立屈伸往返轨迹用于拟合FFG屈伸对侧摆语义的串扰补偿；
+- 默认执行链路与旧版左手G20一致：直接使用最新手套帧并把连续目标交给电机固件，
+  不叠加输入Kalman和输出EMA；两种滤波仍作为显式可选项保留。
+
+状态话题中的 `mapping_mode=factorized_paired_v2` 表示分解式配对已建立；
+`anchor_weights` 只显示当前局部捏合或握持修正强度，不代表整手姿势切换。
+新生成的机械手profile标记为 `mapping_strategy=paired_continuous_v1`，不再保存
+捏合进入/退出阈值或确认帧数；旧profile中的这些字段只为文件兼容而保留，运行时忽略。
+
+实机命令与仿真目标使用两条独立标尺。`cmd_u8` 是电机空间目标，不能反解为真实关节
+弧度；`q_nominal` 由 `actuation_target` 和profile中的URDF名义端点直接计算并限制在
+URDF范围内。状态中的 `simulation_mapping_mode=semantic_urdf_v1` 表示该隔离生效。
+因此，机械手姿势捕获中某个未定义关节的偶然电机值不会改变仿真姿势。
+
+## 1. 构建
+
+```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
+```
+
+## 2. 完整标定FFG
+
+先启动只读FFG节点。它只连接左手套；没有右手套不会退出：
+
+```bash
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p serial_port:=/dev/ttyUSB0 \
+  -p auto_scan:=true
+```
+
+另开终端执行完整标定：
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
+
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --glove-id FFG_LEFT_SN \
+  --operator lxp \
+  --output profiles/glove_FFG_LEFT_SN_left_lxp.json
+```
+
+工具依次采集11个静态姿势，每姿势2秒、重复3次且每次至少50帧；随后采集7组短时往返轨迹。profile保存全部原始帧、每次重复统计、总中位数、MAD和有效帧数。
+
+每次正式启动前做张手、握拳、食指捏合快速佩戴检查：
+
+```bash
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --quick-check profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --quick-output profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json
+```
+
+检查凭据默认12小时有效，并绑定FFG profile的SHA-256。缺失、失败、过期或哈希不匹配时，节点仍发布预览，但拒绝实机使能。
+
+## 3. 建立回放基线与Shadow验证
+
+完整标定前后都建议录制一段包含全部标定手势和5个独立验收手势的FFG基线：
+
+```bash
+ros2 bag record -o bags/ffg_baseline \
+  /ffg/left/raw_joint_state
+```
+
+回放时不要占用FFG串口，使用topic输入运行新链路：
+
+```bash
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p input_mode:=topic \
+  -p raw_input_topic:=/ffg/left/raw_joint_state \
+  -p glove_profile:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  -p g20_profile:=/绝对路径/g20_profile.json \
+  -p o6_profile:=/绝对路径/o6_profile.json
+```
+
+随后执行 `ros2 bag play bags/ffg_baseline`。节点默认不使能，因此只发布两套模型目标，适合逐帧检查长度、名称、范围、连续性、手指局部性和保留通道。
+
+## 4. 标定G20和O6
+
+标定时使用低速、低力矩；不得使用旧手套映射把机械手带到姿势，带电时不得强行手掰。下面的启动会把两台驱动限制为速度30、力矩80，但默认
+`move_on_startup:=false`，不会主动发送张手初始位：
+
+```bash
+ros2 launch linkerhand_retarget ffg_dual_g20_o6.launch.py \
+  auto_scan:=false startup_speed:=30 startup_torque:=80 \
+  move_on_startup:=false
+```
+
+### G20（can0）
+
+终端A启动带命名空间的GUI：
+
+```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
+```
+
+终端B按提示捕获11个姿势：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_capture -- \
+  --model G20 \
+  --seed install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/g20_seed_profile.json \
+  --serial-number G20_LEFT_SN \
+  --operator lxp \
+  --firmware G20_FW \
+  --can-interface can0 \
+  --output profiles/hand_G20_left_G20_LEFT_SN_provisional.json
+```
+
+### O6（can1）
+
+```bash
+ros2 run gui_control gui_control --ros-args \
+  -r __node:=o6_calibration_gui \
+  -p hand_type:=left -p hand_joint:=O6 \
+  -p topic_prefix:=/o6
+```
+
+```bash
+ros2 run linkerhand_retarget hand_pose_capture -- \
+  --model O6 \
+  --seed install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/o6_seed_profile.json \
+  --serial-number O6_LEFT_SN \
+  --operator lxp \
+  --firmware O6_FW \
+  --can-interface can1 \
+  --output profiles/hand_O6_left_O6_LEFT_SN_provisional.json
+```
+
+两个型号的捕获流程都应先把滑块调好、检查通道和方向、等待实机稳定，
+再点击GUI的“保存当前标定姿势”。CLI会自动等待快照，不需要再回终端按Enter；
+快照到达后只需接受默认手势状态或输入实际状态，输入 `r` 可放弃快照并重拍。
+快照和SDK状态都带规范且唯一的通道名称，CLI会按名称重排；缺名、重名或
+型号通道不匹配时拒绝保存。
+
+CLI将GUI保存动作和状态选择共同视为人工确认，并立即写入同目录的
+`*.checkpoint.json`。驱动状态暂时不可用、输入无效状态或选择重拍时，只会重新
+提示当前姿势，不会退出。程序被关闭或异常中断后，使用完全相同的命令
+会校验型号、seed哈希、SN、固件、CAN、操作者和输出路径，并自动跳过已保存姿势。
+若旧检查点中的某些命令超出profile安全范围，CLI会保留其他有效姿势，只移除并
+重拍超限姿势。G20的 `thumb_cmc_yaw` 使用完整0～255命令范围，4个保留通道仍固定
+为255；不得修改保留通道的固定值。
+只有明确希望放弃旧进度时才在原命令末尾增加 `--fresh`；该选项会覆盖旧检查点，
+从第一个姿势重新开始。
+
+O6的桌面、钩拳和不能准确实现的捏合应标为 `approximate`。工具根据张手、桌面、钩拳、握拳捕获值拟合每个主动通道的根部—末端融合权重。只有所有姿势都人工确认，并在最后明确批准，生成的profile才允许申请实机使能。
+
+### 不抄JSON的实机姿势复核
+
+`hand_pose_verify`会直接读取已批准的机械手profile，显示姿势菜单，从SDK当前状态
+按每帧最多4个命令单位平滑过渡到所选姿势，并回读命名状态。每个姿势由操作者选择
+通过、未通过、重放或跳过，结果写入独立的 `*.verification.json`，不会修改标定profile。
+
+复核前必须停止遥操节点和GUI，只保留使用低速、低力矩启动的对应SDK驱动。工具还会
+查询命令话题；发现其他发布者时拒绝开始，运动过程中出现新发布者时停止发送。
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20
+```
+
+只复核一个姿势：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20 \
+  --pose pinch_middle
+```
+
+默认报告与profile同目录、文件名后缀为 `.verification.json`。SDK的 `state_u8`
+只用于判断电机是否接近命令，不是真实关节角GT；最终姿势仍由操作者目视确认。
+
+## 5. 预览、启动和使能
+
+```bash
+ros2 launch linkerhand_retarget ffg_dual_g20_o6.launch.py \
+  serial_port:=/dev/ttyUSB0 auto_scan:=true \
+  glove_profile:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  wear_check:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json \
+  g20_profile:=/绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  o6_profile:=/绝对路径/profiles/hand_O6_left_O6_LEFT_SN_provisional.json \
+  g20_serial_number:=G20_LEFT_SN \
+  o6_serial_number:=O6_LEFT_SN \
+  g20_can:=can0 o6_can:=can1
+```
+
+节点始终默认PREVIEW。确认以下预览话题正确后再使能：
+
+```text
+/ffg/left/raw_joint_state
+/ffg/left/filtered_joint_state
+/retarget/left/hand_intent
+/retarget/left/frame_meta
+/retarget/g20/left/actuation_target
+/retarget/o6/left/actuation_target
+/retarget/g20/left/joint_target_nominal
+/retarget/o6/left/joint_target_nominal
+/retarget/g20/left/cmd_u8_preview
+/retarget/o6/left/cmd_u8_preview
+```
+
+还应确认状态中的对应型号为 `mapping_mode=factorized_paired_v2`。做食指捏合时，
+应主要看到拇指和食指目标变化；缓慢从食指捏合过渡到中指、无名指或小指时，
+目标应连续变化，且不会保持在先前的捏合模板。若行为不符合这两点，不要使能实机。
+
+`raw_joint_state`始终保留未滤波原始数据用于数采；`filtered_joint_state`表示映射器
+实际使用的输入。旧版左手G20直接使用最新手套帧，所以默认关闭可选Kalman：
+
+```text
+input_filter_enabled=false
+input_filter_process_variance=1e-5
+input_filter_measurement_variance=5e-4
+input_filter_reset_gap=0.35
+```
+
+映射器以固定30Hz推进最新目标，即使两次串口更新之间没有新帧也保持连续输出。
+默认把目标直接交给G20固件插值，可由ROS参数修改，不会写回或改变标定profile哈希：
+
+```text
+command_filter_mode=passthrough
+command_filter_ema_alpha=1.0
+command_filter_max_step_u8=255
+command_filter_deadband_u8=0
+```
+
+SDK默认 `repeat_position_commands=true`，因此会像旧版一样在每个30Hz控制心跳重发最新
+位置目标；待发送队列深度仍为1，来不及发送时只保留最新目标，不会累积延迟。
+
+需要处理异常噪声时可显式启用 `input_filter_enabled:=true`，或将
+`command_filter_mode`设为 `ema`；不要同时启用两层滤波，否则小幅连续运动经过取整后
+容易表现为停顿—跳变。
+
+可在不启动ROS和实机的情况下回放profile自带的静态/动态数据：
+
+```bash
+ros2 run linkerhand_retarget retarget_profile_check -- \
+  --glove /绝对路径/glove_profile.json \
+  --robot /绝对路径/hand_G20_profile.json \
+  --model G20 \
+  --output /绝对路径/g20_mapping_quality.json
+```
+
+报告包含有效通道静态复现误差、四种捏合混淆、动态非目标跨度、帧间变化和侧摆串扰
+补偿。它是离线质量诊断，不会移动机械手。
+
+G20驱动默认把每一帧位置命令都视为遥操心跳：只要30Hz控制流仍在，就完全暂停
+同步状态和速度查询；停用遥操且0.2秒没有命令后才恢复查询。运动期间的状态由CAN
+命令响应缓存更新。GUI订阅触发的速度、故障、温度和扭矩诊断读取也遵守同一规则。
+周期性自动清故障默认关闭，需要时仍可通过设置话题手动清除。这样新版需要的状态
+心跳和GUI监控不会用同步CAN查询打断位置控制。启动参数为：
+
+```text
+defer_state_reads_while_commanding=true
+command_quiet_period=0.2
+auto_clear_faults=false
+```
+
+分别或同时使能：
+
+```bash
+ros2 service call /ffg_dual_retarget/enable_g20 std_srvs/srv/SetBool "{data: true}"
+ros2 service call /ffg_dual_retarget/enable_o6 std_srvs/srv/SetBool "{data: true}"
+ros2 service call /ffg_dual_retarget/enable_all std_srvs/srv/SetBool "{data: true}"
+```
+
+软件急停：
+
+```bash
+ros2 service call /ffg_dual_retarget/emergency_stop std_srvs/srv/Trigger "{}"
+```
+
+FFG超过0.35秒没有新帧时，节点停止发布新实机命令并撤销全部使能；任一驱动状态超过1秒时，只撤销对应型号。恢复后必须重新显式使能。
+
+## 6. MuJoCo和Isaac Sim桥
+
+四个仿真桥分别订阅：
+
+```text
+/retarget/g20/left/joint_target_nominal
+/retarget/o6/left/joint_target_nominal
+```
+
+并分别发布：
+
+```text
+/sim/mujoco/g20/left/joint_state
+/sim/mujoco/o6/left/joint_state
+/sim/isaac/g20/left/joint_state
+/sim/isaac/o6/left/joint_state
+```
+
+桥必须使用 `JointState.name` 匹配模型关节，不得按裸下标映射。可在各仿真环境中复用
+`linkerhand_retarget.motion.linkerforce_v2.simulation.reorder_named_target`；
+目标缺少、多出或重复关节名时它会立即抛出 `JointNameMismatch`。本仓库不引入MuJoCo或Isaac的重型依赖，位置控制器仍放在各自仿真环境中。
+
+`q_nominal` 使用 `semantic_urdf_v1`：根部/末端弯曲、侧摆和拇指各语义通道分别在
+URDF名义端点间连续插值，不经过 `cmd_u8` 反解。该目标用于统一驱动和检查仿真模型，
+不是实机测量角度。
+
+仿真验收只比较仿真主动关节与 `q_nominal`，稳定后误差目标为0.02 rad。当前没有Marker或独立角度传感器，因此不得把该误差解释成仿真—实机关节角误差。
+
+## 7. provisional数采
+
+先生成session manifest：
+
+```bash
+ros2 run linkerhand_retarget provisional_manifest -- \
+  --output sessions/demo_001.json \
+  --session-id demo_001 --operator lxp \
+  --glove-profile profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --wear-check profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json \
+  --g20-profile profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --o6-profile profiles/hand_O6_left_O6_LEFT_SN_provisional.json \
+  --g20-urdf src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  --o6-urdf src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/linkerhand_o6_left.urdf \
+  --g20-serial-number G20_LEFT_SN \
+  --o6-serial-number O6_LEFT_SN
+```
+
+工具保存profile/URDF哈希、SN、固件、CAN、话题清单，并打印完整的 `ros2 bag record` 命令。manifest固定写入：
+
+```json
+{
+  "calibration": "provisional",
+  "q_gt": null,
+  "angle_accuracy_claim": false
+}
+```
+
+SDK返回的 `state_u8` 仅作为设备状态记录，不是真实物理角度。
+`/retarget/left/frame_meta` 为每个已映射手套帧记录统一时间戳、
+`sequence`、`calibration=provisional` 和 `q_gt=null`；同一帧产生的
+`hand_intent`、两套 `actuation_target`、`q_nominal` 与 `cmd_u8_preview`
+共享该时间戳。

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

@@ -0,0 +1,156 @@
+{
+  "schema_version": 1,
+  "profile_kind": "robot_hand",
+  "model": "G20",
+  "side": "left",
+  "mapping_strategy": "paired_continuous_v1",
+  "serial_number": "UNSET",
+  "calibration": "provisional",
+  "approved_for_control": false,
+  "metadata": {
+    "description": "G20标定滑块初值，仅用于预览和姿势捕获，禁止实机控制",
+    "firmware": "unknown",
+    "can_interface": "can0",
+    "operator_confirmed": false
+  },
+  "command_length": 20,
+  "command_names": [
+    "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"
+  ],
+  "required_poses": [
+    "open_spread",
+    "fingers_together",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky"
+  ],
+  "poses": {
+    "open_spread": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "fingers_together": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 128, 128, 128, 128, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "tabletop": {
+      "cmd_u8": [255, 0, 0, 0, 0, 255, 128, 128, 128, 128, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "hook": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "fist": {
+      "cmd_u8": [50, 0, 0, 0, 0, 78, 193, 158, 128, 91, 112, 255, 255, 255, 255, 144, 0, 0, 0, 0],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "thumb_max_out": {
+      "cmd_u8": [255, 255, 255, 255, 255, 20, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "thumb_across": {
+      "cmd_u8": [150, 255, 255, 255, 255, 78, 193, 148, 105, 42, 112, 255, 255, 255, 255, 200, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_index": {
+      "cmd_u8": [81, 89, 255, 255, 255, 167, 193, 148, 105, 42, 184, 255, 255, 255, 255, 161, 89, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_middle": {
+      "cmd_u8": [81, 255, 89, 255, 255, 122, 193, 148, 105, 42, 148, 255, 255, 255, 255, 161, 255, 89, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_ring": {
+      "cmd_u8": [81, 255, 255, 89, 255, 78, 193, 148, 105, 42, 112, 255, 255, 255, 255, 161, 255, 255, 89, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_pinky": {
+      "cmd_u8": [81, 255, 255, 255, 89, 50, 193, 148, 105, 42, 90, 255, 255, 255, 255, 161, 255, 255, 255, 89],
+      "state_u8": [],
+      "status": "approximate"
+    }
+  },
+  "command_bounds": [
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
+    [0, 255], [255, 255], [255, 255], [255, 255], [255, 255],
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255]
+  ],
+  "reserved_channels": {
+    "11": 255,
+    "12": 255,
+    "13": 255,
+    "14": 255
+  },
+  "fit_sources": {
+    "thumb_cmc_pitch": ["thumb_root"],
+    "thumb_mcp": ["thumb_tip"],
+    "index_mcp_pitch": ["index_root"],
+    "index_pip": ["index_tip"],
+    "middle_mcp_pitch": ["middle_root"],
+    "middle_pip": ["middle_tip"],
+    "ring_mcp_pitch": ["ring_root"],
+    "ring_pip": ["ring_tip"],
+    "pinky_mcp_pitch": ["pinky_root"],
+    "pinky_pip": ["pinky_tip"]
+  },
+  "channels": [
+    {"index": 0, "name": "thumb_cmc_pitch", "mapping": "monotonic_piecewise", "sources": {"thumb_root": 1.0}, "point_poses": ["open_spread", "thumb_across", "fist"], "points": [[0.0, 255.0], [1.0, 50.0]]},
+    {"index": 1, "name": "index_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"index_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 2, "name": "middle_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"middle_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 3, "name": "ring_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"ring_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 4, "name": "pinky_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"pinky_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 5, "name": "thumb_cmc_roll", "mapping": "piecewise", "sources": {"thumb_abduction": 1.0}, "point_poses": ["thumb_max_out", "open_spread", "thumb_across"], "points": [[0.0, 20.0], [0.5, 255.0], [1.0, 78.0]]},
+    {"index": 6, "name": "index_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"index_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 193.0]]},
+    {"index": 7, "name": "middle_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"middle_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 148.0]]},
+    {"index": 8, "name": "ring_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"ring_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 105.0]]},
+    {"index": 9, "name": "pinky_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"pinky_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 42.0]]},
+    {"index": 10, "name": "thumb_cmc_yaw", "mapping": "piecewise", "sources": {"thumb_rotate": 1.0}, "point_poses": ["thumb_max_out", "open_spread", "thumb_across"], "points": [[0.0, 255.0], [0.5, 255.0], [1.0, 112.0]]},
+    {"index": 15, "name": "thumb_mcp", "mapping": "monotonic_piecewise", "sources": {"thumb_tip": 1.0}, "point_poses": ["open_spread", "thumb_across", "fist"], "points": [[0.0, 255.0], [1.0, 144.0]]},
+    {"index": 16, "name": "index_pip", "mapping": "monotonic_piecewise", "sources": {"index_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 17, "name": "middle_pip", "mapping": "monotonic_piecewise", "sources": {"middle_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 18, "name": "ring_pip", "mapping": "monotonic_piecewise", "sources": {"ring_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 19, "name": "pinky_pip", "mapping": "monotonic_piecewise", "sources": {"pinky_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]}
+  ],
+  "simulation": {
+    "mapping_strategy": "semantic_urdf_v1",
+    "urdf_file": "linkerhand_g20_left.urdf",
+    "urdf_sha256": "51578cb1ece676324983d4076aae6de8d9c623cf437df786f4322134d0c7d305",
+    "joint_names": [
+      "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", "thumb_mcp", "index_pip", "middle_pip", "ring_pip", "pinky_pip"
+    ],
+    "open": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "closed": [0.84, 1.22, 1.22, 1.22, 1.22, 1.4, 0.2, 0.0, -0.2, -0.2, 1.57, 1.26, 1.74, 1.74, 1.74, 1.74],
+    "lower": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "upper": [0.84, 1.22, 1.22, 1.22, 1.22, 1.4, 0.23, 0.23, 0.23, 0.23, 1.57, 1.26, 1.74, 1.74, 1.74, 1.74]
+  },
+  "filter": {
+    "ema_alpha": 1.0,
+    "max_step_u8": 255.0,
+    "max_acceleration_u8_per_frame2": 255.0,
+    "deadband_u8": 0.0,
+    "mode": "passthrough"
+  }
+}

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

@@ -0,0 +1,70 @@
+{
+  "schema_version": 1,
+  "profile_kind": "robot_hand",
+  "model": "O6",
+  "side": "left",
+  "mapping_strategy": "paired_continuous_v1",
+  "serial_number": "UNSET",
+  "calibration": "provisional",
+  "approved_for_control": false,
+  "metadata": {
+    "description": "O6标定滑块初值，仅用于预览和姿势捕获，禁止实机控制",
+    "firmware": "unknown",
+    "can_interface": "can1",
+    "operator_confirmed": false
+  },
+  "command_length": 6,
+  "command_names": ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
+  "required_poses": [
+    "open_spread",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky"
+  ],
+  "poses": {
+    "open_spread": {"cmd_u8": [255, 179, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "tabletop": {"cmd_u8": [255, 179, 64, 64, 64, 64], "state_u8": [], "status": "approximate"},
+    "hook": {"cmd_u8": [255, 179, 96, 96, 96, 96], "state_u8": [], "status": "approximate"},
+    "fist": {"cmd_u8": [0, 151, 0, 0, 0, 0], "state_u8": [], "status": "approximate"},
+    "thumb_max_out": {"cmd_u8": [255, 207, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "thumb_across": {"cmd_u8": [64, 80, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_index": {"cmd_u8": [38, 80, 89, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_middle": {"cmd_u8": [38, 0, 255, 89, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_ring": {"cmd_u8": [38, 0, 255, 255, 89, 255], "state_u8": [], "status": "approximate"},
+    "pinch_pinky": {"cmd_u8": [38, 0, 255, 255, 255, 89], "state_u8": [], "status": "approximate"}
+  },
+  "command_bounds": [[0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255]],
+  "reserved_channels": {},
+  "fit_sources": {
+    "thumb_cmc_pitch": ["thumb_root", "thumb_tip"],
+    "index_mcp_pitch": ["index_root", "index_tip"],
+    "middle_mcp_pitch": ["middle_root", "middle_tip"],
+    "ring_mcp_pitch": ["ring_root", "ring_tip"],
+    "pinky_mcp_pitch": ["pinky_root", "pinky_tip"]
+  },
+  "channels": [
+    {"index": 0, "name": "thumb_cmc_pitch", "mapping": "monotonic_piecewise", "sources": {"thumb_root": 0.5, "thumb_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 255.0], [0.66, 255.0], [1.0, 0.0]]},
+    {"index": 1, "name": "thumb_cmc_yaw", "mapping": "monotonic_piecewise", "sources": {"thumb_opposition": 1.0}, "point_poses": ["open_spread", "thumb_across"], "points": [[0.0, 179.0], [1.0, 80.0]]},
+    {"index": 2, "name": "index_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"index_root": 0.5, "index_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 3, "name": "middle_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"middle_root": 0.5, "middle_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 4, "name": "ring_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"ring_root": 0.5, "ring_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 5, "name": "pinky_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"pinky_root": 0.5, "pinky_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]}
+  ],
+  "simulation": {
+    "mapping_strategy": "semantic_urdf_v1",
+    "urdf_file": "linkerhand_o6_left.urdf",
+    "urdf_sha256": "0aa6cb5fe296389e019c4b575f6559554bb88a7e72c6c3435970434c7f9c6f8e",
+    "joint_names": ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
+    "open": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "closed": [0.58, 1.36, 1.6, 1.6, 1.6, 1.6],
+    "lower": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "upper": [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+  },
+  "filter": {"ema_alpha": 0.55, "max_step_u8": 16.0, "deadband_u8": 0.0}
+}