g20右手标定

.gitignore

@@ -72,6 +72,10 @@ Thumbs.db
 /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_*/

src/g20_thumb_apriltag_calibration/README.md

@@ -1,4 +1,4 @@
-# G20 左手 AprilTag 标定
+# G20 左右手 AprilTag 标定
 
 ## 三机位三维关节轴零位标定（schema v4）
 
@@ -17,16 +17,16 @@ top   = DB2163739，Tag 8/9
 | ID | 机位 | 固定位置/运动件 |
 |---:|---|---|
 | 0 | 正面 | 正面掌壳固定基准 |
-| 1 | 正面 | 拇指 CMC 后连杆 |
+| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
 | 2 | 正面 | 拇指 MCP 后连杆 |
 | 3 | 正面 | 拇指 IP 后末节 |
 | 4 | 侧面 | 掌壳侧面固定基准（最底下） |
-| 5 | 侧面 | 食指 MCP 后连杆 |
-| 6 | 侧面 | 食指 PIP 后连杆 |
-| 7 | 侧面 | 食指 DIP 后末节 |
+| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
+| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
+| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
 | 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
 | 9 | 上面 | 拇指 CMC yaw 运动件 |
-| 10 | 正面 | 食指根部侧摆运动件（index_mcp_roll） |
+| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
 
 ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
 ID 9 必须在拇指横摆的完整行程中持续可见。
@@ -62,7 +62,12 @@ PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒
 
 外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
 一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
-`stereoCalibrate` 联合优化唯一旋转/平移，自动剔除粗大异常组；两组均得到
+`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
@@ -71,10 +76,12 @@ 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不超过1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过
-1.5 mm。文件同时绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存
-通过结果，正式标定也不会运动。
+采集时可分别查看 `/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. 预检和正式标定
 
@@ -83,6 +90,7 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
 ```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 \
@@ -105,12 +113,24 @@ ros2 run image_view image_view --ros-args \
 ```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
@@ -126,66 +146,121 @@ ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
 ```
 
-程序依次扫描正面电机 `0/5/15/6`、侧面 `1/16`、上面 `10`，每项三轮
-`255→0→255`。轨迹角由父/子Tag完整相对四元数的旋转向量投影到三维拟合轴得到。
-对正面/侧面近图像平面的浅圆弧，使用姿态旋转轴约束Tag中心三维圆，只由圆轨迹
-拟合轴线位置和半径，避免单目平面Tag深度噪声改变轴方向；对运动平面充分可观测的
-斜视轨迹，仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
-严格归零：普通通道255，四指侧摆127。中指、无名指、小指继续继承食指曲线；四指
-`mcp_roll/mcp_pitch/pip` 的静态URDF偏移统一固定为0，不再把单根食指的安装残差复制
-到四根手指。`thumb_mcp` 的静态URDF偏移同样固定为0，避免IP轴线深度偏差在命令255
-处产生额外向外伸展；其256项实测轨迹保持不变。
+左手依次扫描电机 `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几何。URDF中确定平行的 `thumb_cmc_pitch→thumb_mcp→thumb_ip`
-和 `index_mcp_pitch→index_pip→index_dip` 使用同轮上游实测方向，同时由各自Tag中心
-轨迹独立拟合轴线位置；被动角度曲线仍由对应父/子Tag完整相对姿态生成。这样保留
-下游轴线对上游零位的约束，同时避免小平面Tag的稳定PnP方向偏差被误认为URDF轴误差。
+零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
+掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
+稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
+进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
+轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
+超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
+Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
+1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
+小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
 
-前两轮拟合，第三轮强制留出验证；第三轮轨迹MAE必须≤1°、P95≤2°，三轮真实
-姿态轴方向极差≤0.75°、轴向/径向RMS≤3 mm。零位留出验收只统计能观测对应上游
-零位的下游轴分量：方向MAE≤1°、P95≤2°，轴线RMS≤3 mm；根轴自身及与当前零位
-无关的固定CAD/PnP残差不会再把零位推向±20°。独立三维运动平面充分可观测的关节
-额外要求姿态轴与圆轴夹角≤1°。任一指标失败会暂停并指出失败关节；调用 `resume`
-只重扫对应电机的6个方向。
+`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留出验证。
 
-零位求解联合优化公共手掌六维位姿和3个拇指CMC直接主动关节的
-`urdf_zero_offset_rad`。依赖链为 yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线
-约束拇指pitch；`thumb_mcp` 固定为0，不再使用IP轴线修改它的静态零位。食指扫描仍
-用于生成四指轨迹，但不再用于修改四指静态URDF零位。
+四指参考源的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。
-偏移超过±20°时整次失败。
+偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
+会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
+不会放宽正式结果的硬门限。
 
-生成修正URDF时，四指主动 `mcp_roll/mcp_pitch/pip` 的关节限位会检查对应256项实测
-轨迹；仅当实测端点越过原始CAD限位时才向外扩展，并额外保留0.5°数值余量。不会
-收窄原限位，也不会修改拇指或四指被动DIP的限位。
+生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
+`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
+JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
 
-任一指标失败时会立即暂停，中文状态显示关节名、实测值和阈值，不再等到42个方向
-全部结束。修正现场问题后调用 `resume`，
-程序只清除该电机任务的内存样本并重扫它的6个方向；前面已通过的关节保留。
-失败样本不从 `raw_samples.jsonl` 删除，而是使用 `attempt` 和 `retry` 记录区分，
-便于调试；最终拟合只使用当前通过尝试的内存数据。
+坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
+重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
+端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
+连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
+暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
+完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
+拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
+只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
+失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
+仍保存在 `raw_samples.jsonl`。
 
-标定食指 `index_mcp_roll`（电机6）时，为避免中指遮挡ID 10，
-程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认
-电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。
-该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把
-食指速度从15降为5；离开该项目后恢复 `[15,15,15,15,15]`。
-侧面标定 `index_mcp_pitch`（电机1）和 `index_pip/index_dip`（电机16）时，
-五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接
-测量关节保持普通速度15。
+每个新机位/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稳定性。离开该标定项后，电机5恢复基准值255；
-最终JSON的 `baseline_command_u8` 不变。
+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秒采集超时。暂停恢复时只要求
@@ -200,16 +275,51 @@ 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)`，绝不叠加旧校准文件或
-覆盖原文件；除16个主动关节的 `origin.rpy` 外，原URDF文本保持不变。每帧Tag SE(3)、
+覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `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`。默认使用单终点连续模式：每个方向只发送一次
@@ -287,9 +397,9 @@ source install/setup.bash
 顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
 
 `1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
-相机节点和三个标定 launch 会自动加载 `config/fastdds_large_images.xml`，使用 64 MB
-共享内存段；否则相机内部虽为 30 Hz，大图订阅端通常只能收到约 1～4 Hz。修改配置后
-必须重启相关 ROS 进程才能生效。
+三相机标定 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；文件缺失时

src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml

@@ -34,6 +34,11 @@ g20_calibration:
     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
@@ -43,6 +48,8 @@ g20_calibration:
     # 轴线一致性仍沿用严格检查。
     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
@@ -51,20 +58,42 @@ g20_calibration:
     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
-    # 电机10在命令0时实测会稳定反馈为3；仅其0端使用±3，255端及其他电机仍为±2。
-    thumb_yaw_zero_endpoint_tolerance_u8: 3.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

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,218 @@
+"""Map G20 u8 commands to URDF joint angles using one 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",
+)
+
+
+class CalibratedCommandMapper:
+    """Validated, side-specific lookup from a G20 command 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")
+        profile = get_hand_calibration_profile(side)
+        self.side = side
+        self.serial_number = str(payload["serial_number"])
+        self._motor_by_joint = {
+            name: int(profile.joint_specs[name].motor_index)
+            for name in G20_URDF_JOINT_NAMES
+        }
+        self._curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name]["angle_rad"]
+            )
+            for name in G20_URDF_JOINT_NAMES
+        }
+
+    @staticmethod
+    def _command_index(value: float) -> int:
+        command = float(value)
+        if not math.isfinite(command):
+            raise ValueError("G20 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 G20_COMMAND_NAMES if name not in by_name]
+            if missing:
+                raise ValueError(
+                    "G20 command is missing named channels: " + ",".join(missing)
+                )
+            command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
+        else:
+            if len(values) != len(G20_COMMAND_NAMES):
+                raise ValueError(
+                    "unnamed G20 command must contain exactly 20 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 G20_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_type", "right")
+        self.declare_parameter("calibration_file", "")
+        self.declare_parameter("input_topic", "")
+        self.declare_parameter("output_topic", "")
+
+        hand_type = str(self.get_parameter("hand_type").value).lower()
+        if hand_type not in {"left", "right"}:
+            raise ValueError("hand_type must be left or right")
+        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
+        )
+        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/g20/{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} G20 calibration for {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(G20_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/extrinsics_node.py

@@ -67,6 +67,8 @@ class StereoCapture:
     transform_candidates: tuple[np.ndarray, ...]
     pair_reprojection_rms_px: float
     skew_ns: int
+    front_reprojection_rms_px: float = float("nan")
+    other_reprojection_rms_px: float = float("nan")
 
 
 @dataclass(frozen=True)
@@ -81,6 +83,64 @@ class StereoFitResult:
     passed: bool
 
 
+def _individual_reprojection_passes(
+    front_rms_px: float,
+    other_rms_px: float,
+    maximum_rms_px: float,
+) -> bool:
+    """Do not let one sharp camera hide a poor observation in the other."""
+    values = np.asarray([front_rms_px, other_rms_px], dtype=float)
+    return bool(
+        np.all(np.isfinite(values))
+        and np.all(values >= 0.0)
+        and np.all(values <= maximum_rms_px)
+    )
+
+
+def _pose_is_novel(
+    pose: np.ndarray,
+    previous_poses: list[np.ndarray],
+    *,
+    minimum_rotation_rad: float,
+    minimum_translation_m: float,
+) -> bool:
+    """Require a new board pose to differ from every accepted pose.
+
+    Comparing only with the most recent capture lets an operator alternate
+    between two almost identical poses and create a large but poorly
+    conditioned data set.  A pose is new when it differs in rotation or
+    translation from every pose already retained.
+    """
+    return all(
+        rotation >= minimum_rotation_rad
+        or translation >= minimum_translation_m
+        for rotation, translation in (
+            _transform_residual(previous, pose) for previous in previous_poses
+        )
+    )
+
+
+def _stereo_fit_failures(
+    result: StereoFitResult,
+    *,
+    minimum_inliers: int,
+    maximum_rms_px: float,
+    maximum_rotation_stability_rad: float,
+    maximum_translation_stability_m: float,
+) -> tuple[str, ...]:
+    """Return stable, user-facing names for failed final quality gates."""
+    failures: list[str] = []
+    if len(result.inlier_indices) < minimum_inliers:
+        failures.append("INLIERS")
+    if result.stereo_rms_px > maximum_rms_px:
+        failures.append("RMS")
+    if result.rotation_stability_rad > maximum_rotation_stability_rad:
+        failures.append("ROT")
+    if result.translation_stability_m > maximum_translation_stability_m:
+        failures.append("TRANS")
+    return tuple(failures)
+
+
 def _select_latest_synchronised_pair(
     front_history: list[Any] | deque[Any],
     other_history: list[Any] | deque[Any],
@@ -313,6 +373,10 @@ def _fit_stereo_robust(
     fit_rms = float("inf")
     transform = np.eye(4)
     per_view = np.full(len(active), float("inf"))
+    # Always compute a provisional fit from three captures onward so the GUI
+    # can guide the operator before the final count is reached.  Remove only
+    # unmistakable geometric/reprojection outliers, and never trim below the
+    # required final inlier count.
     while len(active) >= 3:
         fit_rms, transform, per_view = _stereo_fit_once(
             captures,
@@ -355,10 +419,30 @@ def _fit_stereo_robust(
                 )
             )
         worst_local = int(np.argmax(scores))
-        if scores[worst_local] <= 1.0 or len(active) <= 3:
+        if scores[worst_local] <= 1.0 or len(active) <= minimum_inliers:
             break
         active.pop(worst_local)
 
+    # Moderate bad views (typically 1.2--1.8 px) are not gross outliers, but
+    # accumulating them can keep the batch RMS just above the hard gate
+    # forever.  Trim only a view whose own joint stereo error exceeds the
+    # unchanged final RMS limit, refitting after every removal.  This is
+    # bounded by minimum_inliers and cannot turn an intrinsically poor data set
+    # into a formal result merely by relaxing the threshold.
+    while len(active) > minimum_inliers and fit_rms > maximum_rms_px:
+        worst_local = int(np.argmax(per_view))
+        if float(per_view[worst_local]) <= maximum_rms_px:
+            break
+        active.pop(worst_local)
+        fit_rms, transform, per_view = _stereo_fit_once(
+            captures,
+            active,
+            object_points,
+            front_matrix,
+            other_matrix,
+            image_size,
+        )
+
     rotation_stability = float("inf")
     translation_stability = float("inf")
     if len(active) >= 6:
@@ -389,13 +473,7 @@ def _fit_stereo_robust(
     rejected = tuple(
         index for index in range(len(captures)) if index not in set(active)
     )
-    passed = bool(
-        len(active) >= minimum_inliers
-        and fit_rms <= maximum_rms_px
-        and rotation_stability <= maximum_rotation_stability_rad
-        and translation_stability <= maximum_translation_stability_m
-    )
-    return StereoFitResult(
+    provisional = StereoFitResult(
         front_from_other=transform,
         stereo_rms_px=fit_rms,
         per_view_rms_px=tuple(float(value) for value in per_view),
@@ -403,7 +481,24 @@ def _fit_stereo_robust(
         rejected_indices=rejected,
         rotation_stability_rad=rotation_stability,
         translation_stability_m=translation_stability,
-        passed=passed,
+        passed=False,
+    )
+    failures = _stereo_fit_failures(
+        provisional,
+        minimum_inliers=minimum_inliers,
+        maximum_rms_px=maximum_rms_px,
+        maximum_rotation_stability_rad=maximum_rotation_stability_rad,
+        maximum_translation_stability_m=maximum_translation_stability_m,
+    )
+    return StereoFitResult(
+        front_from_other=provisional.front_from_other,
+        stereo_rms_px=provisional.stereo_rms_px,
+        per_view_rms_px=provisional.per_view_rms_px,
+        inlier_indices=provisional.inlier_indices,
+        rejected_indices=provisional.rejected_indices,
+        rotation_stability_rad=provisional.rotation_stability_rad,
+        translation_stability_m=provisional.translation_stability_m,
+        passed=not failures,
     )
 
 
@@ -510,6 +605,9 @@ class ThreeCameraExtrinsicsNode(Node):
         self.declare_parameter("minimum_captures_per_pair", 15)
         self.declare_parameter("maximum_pair_skew_ms", 100.0)
         self.declare_parameter("maximum_reprojection_rms_px", 1.2)
+        self.declare_parameter(
+            "maximum_single_camera_reprojection_rms_px", 1.5
+        )
         self.declare_parameter("maximum_rotation_repeatability_deg", 0.3)
         self.declare_parameter("maximum_translation_repeatability_m", 0.0015)
         self.declare_parameter("enable_gui", False)
@@ -542,6 +640,9 @@ class ThreeCameraExtrinsicsNode(Node):
         self.maximum_reprojection_rms_px = float(
             value("maximum_reprojection_rms_px")
         )
+        self.maximum_single_camera_reprojection_rms_px = float(
+            value("maximum_single_camera_reprojection_rms_px")
+        )
         self.maximum_rotation_repeatability_rad = math.radians(
             float(value("maximum_rotation_repeatability_deg"))
         )
@@ -567,6 +668,16 @@ class ThreeCameraExtrinsicsNode(Node):
             raise ValueError("checkerboard geometry is invalid")
         if self.minimum_captures < 15:
             raise ValueError("minimum_captures_per_pair must be at least 15")
+        if self.maximum_reprojection_rms_px <= 0.0:
+            raise ValueError("maximum_reprojection_rms_px must be positive")
+        if (
+            self.maximum_single_camera_reprojection_rms_px
+            < self.maximum_reprojection_rms_px
+        ):
+            raise ValueError(
+                "maximum_single_camera_reprojection_rms_px must be at "
+                "least maximum_reprojection_rms_px"
+            )
         if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0:
             raise ValueError("gui_refresh_hz must be in (0, 10]")
         if self.auto_capture_stable_seconds < 0.5:
@@ -753,13 +864,20 @@ class ThreeCameraExtrinsicsNode(Node):
             "skew_ns": skew,
             "pair_reprojection_rms_px": pair_rms,
         }
-        if pair_rms > self.maximum_reprojection_rms_px:
+        camera_rms_failed = not _individual_reprojection_passes(
+            front.reprojection_rms_px,
+            selected.reprojection_rms_px,
+            self.maximum_single_camera_reprojection_rms_px,
+        )
+        if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px:
             return PairAssessment(
                 ready=False,
                 reason_code="reprojection_rms_too_large",
                 message=(
-                    f"当前组重投影RMS={pair_rms:.3f}px超过"
-                    f"{self.maximum_reprojection_rms_px:.3f}px门限，未计入。"
+                    f"当前组未通过采集门限，未计入。单相机上限="
+                    f"{self.maximum_single_camera_reprojection_rms_px:.3f}px，"
+                    f"组合上限={self.maximum_reprojection_rms_px:.3f}px；"
+                    f"组合RMS={pair_rms:.3f}px，"
                     f"front={front.reprojection_rms_px:.3f}px，"
                     f"{other}={selected.reprojection_rms_px:.3f}px；"
                     f"已保留{len(self.captures[other])}/{self.minimum_captures}组"
@@ -775,19 +893,11 @@ class ThreeCameraExtrinsicsNode(Node):
         previous = self.captures[other]
         board_poses = self.captured_front_board[other]
         if board_poses:
-            board_change_rotation = (
-                Rotation.from_matrix(board_poses[-1][:3, :3]).inv()
-                * Rotation.from_matrix(
-                    front.camera_from_board_candidates[0][:3, :3]
-                )
-            ).magnitude()
-            board_change_translation = np.linalg.norm(
-                board_poses[-1][:3, 3]
-                - front.camera_from_board_candidates[0][:3, 3]
-            )
-            if (
-                board_change_rotation < math.radians(2.0)
-                and board_change_translation < 0.015
+            if not _pose_is_novel(
+                front.camera_from_board_candidates[0],
+                board_poses,
+                minimum_rotation_rad=math.radians(2.0),
+                minimum_translation_m=0.015,
             ):
                 return PairAssessment(
                     ready=False,
@@ -891,6 +1001,12 @@ class ThreeCameraExtrinsicsNode(Node):
                     assessment.pair_reprojection_rms_px
                 ),
                 skew_ns=assessment.skew_ns,
+                front_reprojection_rms_px=(
+                    assessment.front_pose.reprojection_rms_px
+                ),
+                other_reprojection_rms_px=(
+                    assessment.selected_pose.reprojection_rms_px
+                ),
             )
         )
         self.captured_front_board[other].append(
@@ -968,12 +1084,17 @@ class ThreeCameraExtrinsicsNode(Node):
                 True,
             )
         rms = "--" if pose is None else f"{pose.reprojection_rms_px:.3f}px"
-        color = (
-            (40, 220, 40)
-            if pose is not None
-            and pose.reprojection_rms_px <= self.maximum_reprojection_rms_px
-            else (0, 180, 255)
-        )
+        color = (0, 180, 255)
+        if pose is not None:
+            if pose.reprojection_rms_px <= self.maximum_reprojection_rms_px:
+                color = (40, 220, 40)
+            elif (
+                pose.reprojection_rms_px
+                <= self.maximum_single_camera_reprojection_rms_px
+            ):
+                color = (0, 210, 255)
+            else:
+                color = (0, 0, 255)
         cv2.putText(
             image,
             f"{label.upper()}  RMS {rms}",
@@ -1150,6 +1271,10 @@ class ThreeCameraExtrinsicsNode(Node):
             else:
                 self.gui_auto_pose = None
                 self.gui_auto_stable_since = None
+            # Auto capture can update the fit during this callback.  Render
+            # the new result immediately instead of showing stale metrics for
+            # one GUI period.
+            active_fit = self.fit_results[self.gui_active_other]
             front_frame = assessment.front_frame
             selected_frame = assessment.selected_frame
             if front_frame is None and self.image_history["front"]:
@@ -1174,11 +1299,24 @@ class ThreeCameraExtrinsicsNode(Node):
             canvas[10 : 10 + view_height, 655 : 655 + view_width] = right
 
             ready_color = (40, 220, 40) if assessment.ready else (0, 80, 230)
-            batch_status = (
-                " | BATCH PASSED"
-                if active_fit is not None and active_fit.passed
-                else ""
-            )
+            batch_status = ""
+            if active_fit is not None:
+                failures = _stereo_fit_failures(
+                    active_fit,
+                    minimum_inliers=self.minimum_captures,
+                    maximum_rms_px=self.maximum_reprojection_rms_px,
+                    maximum_rotation_stability_rad=(
+                        self.maximum_rotation_repeatability_rad
+                    ),
+                    maximum_translation_stability_m=(
+                        self.maximum_translation_repeatability_m
+                    ),
+                )
+                batch_status = (
+                    " | BATCH PASSED"
+                    if not failures
+                    else " | BATCH FAIL: " + ",".join(failures)
+                )
             cv2.putText(
                 canvas,
                 self._gui_reason_text(assessment) + batch_status,
@@ -1199,27 +1337,31 @@ class ThreeCameraExtrinsicsNode(Node):
                 if assessment.skew_ns is None
                 else f"{assessment.skew_ns / 1e6:.1f}"
             )
+            batch_rms = (
+                "--" if active_fit is None else f"{active_fit.stereo_rms_px:.3f}"
+            )
             rotation = (
                 "--"
-                if assessment.provisional_rotation_error_rad is None
-                else f"{math.degrees(assessment.provisional_rotation_error_rad):.3f}"
+                if active_fit is None
+                else f"{math.degrees(active_fit.rotation_stability_rad):.3f}"
             )
             translation = (
                 "--"
-                if assessment.provisional_translation_error_m is None
-                else f"{assessment.provisional_translation_error_m * 1000.0:.3f}"
+                if active_fit is None
+                else f"{active_fit.translation_stability_m * 1000.0:.3f}"
             )
             cv2.putText(
                 canvas,
                 f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px  "
                 f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms  "
-                f"Batch stability {rotation}/"
+                f"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px  "
+                f"stability {rotation}/"
                 f"{math.degrees(self.maximum_rotation_repeatability_rad):.3f}deg  "
                 f"{translation}/"
                 f"{self.maximum_translation_repeatability_m * 1000.0:.3f}mm",
                 (20, 562),
                 cv2.FONT_HERSHEY_SIMPLEX,
-                0.58,
+                0.52,
                 (220, 220, 220),
                 1,
                 cv2.LINE_AA,
@@ -1326,15 +1468,31 @@ class ThreeCameraExtrinsicsNode(Node):
             results[view] = result
         if any(not result.passed for result in results.values()):
             response.success = False
+            diagnostics: list[str] = []
+            for view, result in results.items():
+                failures = _stereo_fit_failures(
+                    result,
+                    minimum_inliers=self.minimum_captures,
+                    maximum_rms_px=self.maximum_reprojection_rms_px,
+                    maximum_rotation_stability_rad=(
+                        self.maximum_rotation_repeatability_rad
+                    ),
+                    maximum_translation_stability_m=(
+                        self.maximum_translation_repeatability_m
+                    ),
+                )
+                diagnostics.append(
+                    f"{view}: RMS={result.stereo_rms_px:.3f}px, "
+                    f"内点={len(result.inlier_indices)}/"
+                    f"{len(self.stereo_captures[view])}, "
+                    f"旋转稳定性="
+                    f"{math.degrees(result.rotation_stability_rad):.3f}°, "
+                    f"平移稳定性="
+                    f"{result.translation_stability_m * 1000.0:.3f}mm, "
+                    f"未通过={','.join(failures) or '无'}"
+                )
             response.message = "联合外参质量不合格：" + "; ".join(
-                f"{view}: RMS={result.stereo_rms_px:.3f}px, "
-                f"内点={len(result.inlier_indices)}/"
-                f"{len(self.stereo_captures[view])}, "
-                f"旋转稳定性="
-                f"{math.degrees(result.rotation_stability_rad):.3f}°, "
-                f"平移稳定性="
-                f"{result.translation_stability_m * 1000.0:.3f}mm"
-                for view, result in results.items()
+                diagnostics
             )
             return response
         transforms = {

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py

@@ -72,132 +72,249 @@ class JointCurveFit:
     zero_offset_rad: float = 0.0
 
 
-VIEW_TAGS: dict[str, dict[str, int]] = {
-    "front": {
-        "front_base": 0,
-        "thumb_cmc": 1,
-        "thumb_mcp": 2,
-        "thumb_ip": 3,
-        "index_roll": 10,
-    },
-    "side": {
-        "side_base": 4,
-        "index_mcp": 5,
-        "index_pip": 6,
-        "index_dip": 7,
-    },
-    "top": {
-        "top_base": 8,
-        "thumb_yaw": 9,
-    },
+@dataclass(frozen=True)
+class HandCalibrationProfile:
+    side: str
+    reference_finger: str
+    view_tags: Mapping[str, Mapping[str, int]]
+    preflight_view_roles: Mapping[str, tuple[str, ...]]
+    joint_specs: Mapping[str, JointSpec]
+    sweep_specs: tuple[SweepSpec, ...]
+    image_trajectory_joints: frozenset[str]
+    roll_clearance_commands: Mapping[int, int]
+    thumb_pitch_clearance_commands: Mapping[int, int]
+
+    @property
+    def measured_joints(self) -> tuple[str, ...]:
+        return tuple(
+            name for name, spec in self.joint_specs.items() if spec.measured
+        )
+
+    @property
+    def active_joints(self) -> tuple[str, ...]:
+        return tuple(
+            name for name, spec in self.joint_specs.items() if spec.active
+        )
+
+    @property
+    def passive_joints(self) -> tuple[str, ...]:
+        return tuple(
+            name for name, spec in self.joint_specs.items() if not spec.active
+        )
+
+    @property
+    def reference_roll_motor(self) -> int:
+        return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index)
+
+    @property
+    def reference_pitch_motor(self) -> int:
+        return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index)
+
+    @property
+    def reference_pip_motor(self) -> int:
+        return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index)
+
+    @property
+    def reference_speed_slot(self) -> int:
+        return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[
+            self.reference_finger
+        ]
+
+
+_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky")
+_MOTOR_BY_JOINT: dict[str, int] = {
+    **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
+    **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
+    **{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)},
 }
 
 
-JOINT_SPECS: dict[str, JointSpec] = {
-    "thumb_cmc_pitch": JointSpec(
-        "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc",
+def _build_hand_profile(side: str) -> HandCalibrationProfile:
+    hand_side = str(side).lower()
+    if hand_side not in {"left", "right"}:
+        raise ValueError("hand side must be left or right")
+    reference = "index" if hand_side == "left" else "pinky"
+    roll_role = f"{reference}_roll"
+    mcp_role = f"{reference}_mcp"
+    pip_role = f"{reference}_pip"
+    dip_role = f"{reference}_dip"
+    view_tags: dict[str, dict[str, int]] = {
+        "front": {
+            "front_base": 0,
+            "thumb_cmc": 1,
+            "thumb_mcp": 2,
+            "thumb_ip": 3,
+            roll_role: 10,
+        },
+        "side": {
+            "side_base": 4,
+            mcp_role: 5,
+            pip_role: 6,
+            dip_role: 7,
+        },
+        "top": {"top_base": 8, "thumb_yaw": 9},
+    }
+    preflight_view_roles = {
+        view: tuple(tags) for view, tags in view_tags.items()
+    }
+    joint_specs: dict[str, JointSpec] = {}
+    # Both hands use the front base/Tag 1 pair for motor 0.  Keeping the
+    # complete thumb root chain in one calibrated view avoids introducing a
+    # cross-camera axis-line phase into the URDF zero solve.
+    thumb_pitch_view = "front"
+    thumb_pitch_parent = "front_base"
+    joint_specs["thumb_cmc_pitch"] = JointSpec(
+        "thumb_cmc_pitch",
+        0,
+        True,
+        thumb_pitch_view,
+        thumb_pitch_parent,
+        "thumb_cmc",
         zero_kind="urdf_axis_chain",
-    ),
-    "index_mcp_pitch": JointSpec(
-        "index_mcp_pitch", 1, True, "side", "side_base", "index_mcp",
-        zero_kind="urdf_axis_chain",
-    ),
-    "middle_mcp_pitch": JointSpec(
-        "middle_mcp_pitch", 2, True, None, None, None,
-        source_joint="index_mcp_pitch", zero_kind="inherited",
-    ),
-    "ring_mcp_pitch": JointSpec(
-        "ring_mcp_pitch", 3, True, None, None, None,
-        source_joint="index_mcp_pitch", zero_kind="inherited",
-    ),
-    "pinky_mcp_pitch": JointSpec(
-        "pinky_mcp_pitch", 4, True, None, None, None,
-        source_joint="index_mcp_pitch", zero_kind="inherited",
-    ),
-    "thumb_cmc_roll": JointSpec(
+    )
+    for finger in _FINGERS:
+        name = f"{finger}_mcp_pitch"
+        source = None if finger == reference else f"{reference}_mcp_pitch"
+        joint_specs[name] = JointSpec(
+            name,
+            _MOTOR_BY_JOINT[name],
+            True,
+            "side" if source is None else None,
+            "side_base" if source is None else None,
+            mcp_role if source is None else None,
+            source_joint=source,
+            zero_kind="urdf_axis_chain" if source is None else "inherited",
+        )
+    joint_specs["thumb_cmc_roll"] = JointSpec(
         "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
         zero_kind="urdf_axis_chain",
-    ),
-    "index_mcp_roll": JointSpec(
-        "index_mcp_roll", 6, True, "front", "front_base", "index_roll",
-        zero_kind="urdf_axis_chain",
-    ),
-    "middle_mcp_roll": JointSpec(
-        "middle_mcp_roll", 7, True, None, None, None,
-        source_joint="index_mcp_roll", zero_kind="inherited",
-    ),
-    "ring_mcp_roll": JointSpec(
-        "ring_mcp_roll", 8, True, None, None, None,
-        source_joint="index_mcp_roll", zero_kind="inherited",
-    ),
-    "pinky_mcp_roll": JointSpec(
-        "pinky_mcp_roll", 9, True, None, None, None,
-        source_joint="index_mcp_roll", zero_kind="inherited",
-    ),
-    "thumb_cmc_yaw": JointSpec(
+    )
+    for finger in _FINGERS:
+        name = f"{finger}_mcp_roll"
+        source = None if finger == reference else f"{reference}_mcp_roll"
+        joint_specs[name] = JointSpec(
+            name,
+            _MOTOR_BY_JOINT[name],
+            True,
+            "front" if source is None else None,
+            "front_base" if source is None else None,
+            roll_role if source is None else None,
+            source_joint=source,
+            zero_kind="urdf_axis_chain" if source is None else "inherited",
+        )
+    joint_specs["thumb_cmc_yaw"] = JointSpec(
         "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
         zero_kind="urdf_axis_chain",
-    ),
-    "thumb_mcp": JointSpec(
+    )
+    joint_specs["thumb_mcp"] = JointSpec(
         "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
         zero_kind="urdf_axis_chain",
-    ),
-    "index_pip": JointSpec(
-        "index_pip", 16, True, "side", "index_mcp", "index_pip",
-        zero_kind="urdf_axis_chain",
-    ),
-    "middle_pip": JointSpec(
-        "middle_pip", 17, True, None, None, None,
-        source_joint="index_pip", zero_kind="inherited",
-    ),
-    "ring_pip": JointSpec(
-        "ring_pip", 18, True, None, None, None,
-        source_joint="index_pip", zero_kind="inherited",
-    ),
-    "pinky_pip": JointSpec(
-        "pinky_pip", 19, True, None, None, None,
-        source_joint="index_pip", zero_kind="inherited",
-    ),
-    "thumb_ip": JointSpec(
+    )
+    for finger in _FINGERS:
+        name = f"{finger}_pip"
+        source = None if finger == reference else f"{reference}_pip"
+        joint_specs[name] = JointSpec(
+            name,
+            _MOTOR_BY_JOINT[name],
+            True,
+            "side" if source is None else None,
+            mcp_role if source is None else None,
+            pip_role if source is None else None,
+            source_joint=source,
+            zero_kind="urdf_axis_chain" if source is None else "inherited",
+        )
+    joint_specs["thumb_ip"] = JointSpec(
         "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
-    ),
-    "index_dip": JointSpec(
-        "index_dip", 16, False, "side", "index_pip", "index_dip",
-    ),
-    "middle_dip": JointSpec(
-        "middle_dip", 17, False, None, None, None,
-        source_joint="index_dip",
-    ),
-    "ring_dip": JointSpec(
-        "ring_dip", 18, False, None, None, None,
-        source_joint="index_dip",
-    ),
-    "pinky_dip": JointSpec(
-        "pinky_dip", 19, False, None, None, None,
-        source_joint="index_dip",
-    ),
-}
+    )
+    for finger in _FINGERS:
+        name = f"{finger}_dip"
+        source = None if finger == reference else f"{reference}_dip"
+        joint_specs[name] = JointSpec(
+            name,
+            _MOTOR_BY_JOINT[f"{finger}_pip"],
+            False,
+            "side" if source is None else None,
+            pip_role if source is None else None,
+            dip_role if source is None else None,
+            source_joint=source,
+        )
+    sweep_specs = (
+        SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)),
+        SweepSpec("front", 5, ("thumb_cmc_roll",)),
+        SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
+        SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], (
+            f"{reference}_mcp_roll",
+        )),
+        SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], (
+            f"{reference}_mcp_pitch",
+        )),
+        SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], (
+            f"{reference}_pip", f"{reference}_dip",
+        )),
+        SweepSpec("top", 10, ("thumb_cmc_yaw",)),
+    )
+    image_joints = frozenset(
+        {
+            "thumb_cmc_pitch",
+            "thumb_cmc_roll",
+            "thumb_mcp",
+            "thumb_ip",
+            f"{reference}_mcp_roll",
+            f"{reference}_mcp_pitch",
+            f"{reference}_pip",
+        }
+    )
+    measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"]
+    # The roll channels are mirrored mechanically between hands.  Command 0
+    # moves the three non-reference fingers to the camera-right clearance pose
+    # on the left hand, while the same physical pose is command 255 on the
+    # right hand.  Using 0 for both sides made the right index/middle/ring
+    # fingers lean left and could obstruct or load the pinky roll sweep.
+    roll_clearance_command = 0 if hand_side == "left" else 255
+    clearance = {
+        _MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command
+        for finger in _FINGERS
+        if finger != reference
+    }
+    assert measured_roll not in clearance
+    # Motor 0 is observed from the front with both other thumb-root channels
+    # fully open.  Store the commands explicitly for the right hand so a
+    # caller-provided baseline cannot silently restore the obsolete 50/120
+    # pose and hide Tag 1 from the front camera.
+    thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {}
+    return HandCalibrationProfile(
+        side=hand_side,
+        reference_finger=reference,
+        view_tags=view_tags,
+        preflight_view_roles=preflight_view_roles,
+        joint_specs=joint_specs,
+        sweep_specs=sweep_specs,
+        image_trajectory_joints=image_joints,
+        roll_clearance_commands=clearance,
+        thumb_pitch_clearance_commands=thumb_pitch_clearance,
+    )
 
 
-SWEEP_SPECS: tuple[SweepSpec, ...] = (
-    SweepSpec("front", 0, ("thumb_cmc_pitch",)),
-    SweepSpec("front", 5, ("thumb_cmc_roll",)),
-    SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
-    SweepSpec("front", 6, ("index_mcp_roll",)),
-    SweepSpec("side", 1, ("index_mcp_pitch",)),
-    SweepSpec("side", 16, ("index_pip", "index_dip")),
-    SweepSpec("top", 10, ("thumb_cmc_yaw",)),
-)
+LEFT_HAND_PROFILE = _build_hand_profile("left")
+RIGHT_HAND_PROFILE = _build_hand_profile("right")
 
-MEASURED_JOINTS: tuple[str, ...] = tuple(
-    name for name, spec in JOINT_SPECS.items() if spec.measured
-)
-ACTIVE_JOINTS: tuple[str, ...] = tuple(
-    name for name, spec in JOINT_SPECS.items() if spec.active
-)
-PASSIVE_JOINTS: tuple[str, ...] = tuple(
-    name for name, spec in JOINT_SPECS.items() if not spec.active
-)
+
+def get_hand_calibration_profile(side: str) -> HandCalibrationProfile:
+    value = str(side).lower()
+    if value == "left":
+        return LEFT_HAND_PROFILE
+    if value == "right":
+        return RIGHT_HAND_PROFILE
+    raise ValueError("hand side must be left or right")
+
+
+# Backwards-compatible aliases keep the existing left-hand API stable.
+VIEW_TAGS = LEFT_HAND_PROFILE.view_tags
+JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs
+SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs
+MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints
+ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints
+PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints
 SPLAY_JOINTS: tuple[str, ...] = (
     "index_mcp_roll",
     "middle_mcp_roll",
@@ -213,25 +330,11 @@ SPLAY_JOINTS: tuple[str, ...] = (
 # side-view index_dip and oblique top-view yaw remain parent-relative full
 # SE(3) measurements; their circle directions are constrained by the more
 # repeatable relative-orientation screw axis in urdf_zero.py.
-IMAGE_TRAJECTORY_JOINTS: frozenset[str] = frozenset(
-    {
-        "thumb_cmc_pitch",
-        "thumb_cmc_roll",
-        "thumb_mcp",
-        "thumb_ip",
-        "index_mcp_roll",
-        "index_mcp_pitch",
-        "index_pip",
-    }
-)
+IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints
 
 # Keep the three unmeasured finger-roll motors away from the front camera's
 # line of sight while index_mcp_roll is measured.
-INDEX_ROLL_CLEARANCE_COMMANDS: dict[int, int] = {
-    7: 0,
-    8: 0,
-    9: 0,
-}
+INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands
 
 # Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
 # measured.  This keeps the moving top-view tag sufficiently front-facing.
@@ -240,10 +343,16 @@ THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
 }
 
 
-def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]:
+def calibration_auxiliary_commands(
+    spec: SweepSpec,
+    *,
+    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
+) -> dict[int, int]:
     """Return motors that must remain fixed throughout one calibration task."""
-    if spec.motor_index == 6:
-        return dict(INDEX_ROLL_CLEARANCE_COMMANDS)
+    if spec.motor_index == 0:
+        return dict(profile.thumb_pitch_clearance_commands)
+    if spec.motor_index == profile.reference_roll_motor:
+        return dict(profile.roll_clearance_commands)
     if spec.motor_index == 10:
         return dict(THUMB_YAW_CLEARANCE_COMMANDS)
     return {}
@@ -253,11 +362,12 @@ def build_full_hand_command(
     motor_index: int,
     command_u8: int,
     baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
+    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
 ) -> list[int]:
     if len(baseline) != 20:
         raise ValueError("baseline must contain exactly 20 values")
     motor = int(motor_index)
-    if motor not in {spec.motor_index for spec in JOINT_SPECS.values()}:
+    if motor not in {spec.motor_index for spec in profile.joint_specs.values()}:
         raise ValueError("motor_index is not a controlled G20 calibration channel")
     command = int(command_u8)
     if not 0 <= command <= 255:
@@ -273,15 +383,17 @@ def build_calibration_motion_command(
     spec: SweepSpec,
     command_u8: int,
     baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
+    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
 ) -> list[int]:
     """Build a sweep command, including any required clearance pose."""
     result = build_full_hand_command(
         spec.motor_index,
         command_u8,
         baseline=baseline,
+        profile=profile,
     )
     for motor_index, auxiliary_command in calibration_auxiliary_commands(
-        spec
+        spec, profile=profile
     ).items():
         result[motor_index] = auxiliary_command
     return result
@@ -293,6 +405,7 @@ def build_calibration_speed_profile(
     normal_speed: int,
     index_roll_speed: int,
     index_flex_speed: int,
+    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
 ) -> list[int]:
     """Return G20's five per-finger speeds for one calibration task."""
     normal = int(normal_speed)
@@ -303,10 +416,13 @@ def build_calibration_speed_profile(
     ):
         raise ValueError("calibration speeds must be in [0, 255]")
     speeds = [normal] * 5
-    if spec.motor_index == 6:
-        speeds[1] = index_roll
-    elif spec.motor_index in {1, 16}:
-        speeds[1] = index_flex
+    if spec.motor_index == profile.reference_roll_motor:
+        speeds[profile.reference_speed_slot] = index_roll
+    elif spec.motor_index in {
+        profile.reference_pitch_motor,
+        profile.reference_pip_motor,
+    }:
+        speeds[profile.reference_speed_slot] = index_flex
     return speeds
 
 
@@ -487,9 +603,10 @@ def fit_measured_joint_curve(
     image_maximum_radial_rms_px: float = 2.0,
     image_maximum_radial_p95_px: float = 3.5,
     image_minimum_radius_px: float = 20.0,
+    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
 ) -> JointCurveFit:
     """Select the stable trajectory representation for one measured joint."""
-    if str(joint_name) in IMAGE_TRAJECTORY_JOINTS:
+    if str(joint_name) in profile.image_trajectory_joints:
         return fit_joint_image_curve(
             records,
             maximum_radial_rms_px=image_maximum_radial_rms_px,
@@ -624,11 +741,13 @@ def build_compact_payload(
     validation_errors_rad: Sequence[float],
     passed: bool,
     baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
+    side: str = "left",
 ) -> dict[str, Any]:
-    if set(measured_fits) != set(MEASURED_JOINTS):
+    profile = get_hand_calibration_profile(side)
+    if set(measured_fits) != set(profile.measured_joints):
         raise ValueError("measured_fits must contain all directly measured joints")
     expected_active = {
-        name for name, spec in JOINT_SPECS.items() if spec.active
+        name for name, spec in profile.joint_specs.items() if spec.active
     }
     if set(urdf_zero_offsets_rad) != expected_active:
         raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set")
@@ -636,7 +755,7 @@ def build_compact_payload(
         raise ValueError("baseline must contain exactly 20 commands")
 
     joints: dict[str, dict[str, Any]] = {}
-    for name, spec in JOINT_SPECS.items():
+    for name, spec in profile.joint_specs.items():
         source_name = spec.source_joint or name
         fit = measured_fits[source_name]
         joint: dict[str, Any] = {
@@ -662,7 +781,7 @@ def build_compact_payload(
     payload = {
         "schema_version": 4,
         "model": "G20",
-        "side": "left",
+        "side": profile.side,
         "serial_number": str(serial_number),
         "angle_unit": "rad",
         "command_range": [0, 255],
@@ -691,18 +810,19 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
         raise ValueError("compact calibration has unexpected top-level fields")
     if payload["schema_version"] != 4:
         raise ValueError("schema_version must be 4")
-    if payload["model"] != "G20" or payload["side"] != "left":
-        raise ValueError("payload must describe a left G20")
+    if payload["model"] != "G20" or payload["side"] not in {"left", "right"}:
+        raise ValueError("payload must describe a left or right G20")
+    profile = get_hand_calibration_profile(str(payload["side"]))
     if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
         raise ValueError("payload angle or command units are invalid")
     baseline = payload["baseline_command_u8"]
     if not isinstance(baseline, list) or len(baseline) != 20:
         raise ValueError("baseline_command_u8 must contain 20 values")
     joints = payload["joints"]
-    if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS):
+    if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs):
         raise ValueError("payload must contain exactly 21 G20 joints")
 
-    for name, spec in JOINT_SPECS.items():
+    for name, spec in profile.joint_specs.items():
         joint = joints[name]
         allowed = {"motor_index", "angle_rad"}
         allowed.add("zero_command_u8" if spec.active else "passive")
@@ -747,8 +867,9 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
                 raise ValueError(f"{name} must copy its source curve exactly")
             if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
                 raise ValueError(f"{name} must copy its source zero command")
-            if spec.active and joint["zero_angles"] != source["zero_angles"]:
-                raise ValueError(f"{name} must copy its source zero offset")
+            # source_joint denotes a dynamic command-angle curve source.  The
+            # absolute URDF zero belongs to an independent motor/assembly and
+            # must remain per-joint unless it was independently observed.
 
     quality = payload["quality"]
     if set(quality) != {

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -0,0 +1,846 @@
+"""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"))
+    return dict(payload["g20_calibration"]["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]),
+    )
+
+
+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)
+    extrinsics = load_three_camera_extrinsics(extrinsics_file)
+    cache: dict[tuple[str, int], JointAxisMeasurement] = {}
+    reference = profile.reference_finger
+    upstream_by_joint = {
+        "thumb_mcp": "thumb_cmc_pitch",
+        "thumb_ip": "thumb_mcp",
+        f"{reference}_pip": f"{reference}_mcp_pitch",
+        f"{reference}_dip": f"{reference}_pip",
+    }
+
+    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,
+        )
+        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)
+    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:
+                    failures.append(
+                        f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}"
+                    )
+                    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)
+    side = str(start["hand_type"]).lower()
+    profile = get_hand_calibration_profile(side)
+    zero_profile = get_zero_calibration_profile(side)
+    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,
+    }
+    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,
+    )
+    validate_compact_payload(payload)
+
+    stamp = session.name
+    final_json = session / (
+        f"g20_{side}_{hand_serial}_calibration{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"g20_{side}_{hand_serial}_offline_validation{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),
+        "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 G20 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/pnp.py

@@ -41,6 +41,19 @@ def _relative_pose(
     return relative_rotation, relative_translation
 
 
+def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float:
+    """Return the undirected angle between two observed Tag face normals."""
+    first_normal = Rotation.from_quat(first.quaternion_xyzw).apply(
+        [0.0, 0.0, 1.0]
+    )
+    second_normal = Rotation.from_quat(second.quaternion_xyzw).apply(
+        [0.0, 0.0, 1.0]
+    )
+    return math.acos(
+        abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0)))
+    )
+
+
 def select_rigid_group_trajectory(
     frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
     *,
@@ -50,6 +63,8 @@ def select_rigid_group_trajectory(
     rotation_scale_rad: float,
     translation_scale_m: float,
     pair_geometry: str = "pose",
+    normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+    normal_alignment_scale_rad: float | None = None,
 ) -> tuple[
     list[dict[str, SquareTagPose]],
     dict[str, float | str],
@@ -66,20 +81,34 @@ def select_rigid_group_trajectory(
     """
     role_names = tuple(str(role) for role in roles)
     pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    normal_pair_names = tuple(
+        (str(first), str(second))
+        for first, second in normal_alignment_pairs
+    )
     if not frames:
         raise ValueError("at least one PnP frame is required")
     if len(set(role_names)) != len(role_names) or not role_names:
         raise ValueError("roles must be non-empty and unique")
     if any(
         parent not in role_names or child not in role_names
-        for parent, child in pair_names
+        for parent, child in (*pair_names, *normal_pair_names)
     ):
-        raise ValueError("fixed_pairs must reference roles")
+        raise ValueError("geometry pairs must reference roles")
     reprojection_scale = float(reprojection_scale_px)
     rotation_scale = float(rotation_scale_rad)
     translation_scale = float(translation_scale_m)
     geometry_mode = str(pair_geometry)
-    if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0:
+    normal_scale = (
+        rotation_scale
+        if normal_alignment_scale_rad is None
+        else float(normal_alignment_scale_rad)
+    )
+    if min(
+        reprojection_scale,
+        rotation_scale,
+        translation_scale,
+        normal_scale,
+    ) <= 0.0:
         raise ValueError("trajectory selection scales must be positive")
     if geometry_mode not in {"pose", "distance"}:
         raise ValueError("pair_geometry must be pose or distance")
@@ -110,6 +139,12 @@ def select_rigid_group_trajectory(
             pose.reprojection_error_px
             for pose in combination.values()
         ) / reprojection_scale
+        normal_alignment_cost = sum(
+            _normal_alignment_rad(
+                combination[first], combination[second]
+            )
+            for first, second in normal_pair_names
+        ) / normal_scale
         rotation_drifts: list[float] = []
         translation_drifts: list[float] = []
         distance_drifts: list[float] = []
@@ -156,7 +191,7 @@ def select_rigid_group_trajectory(
                 + sum(translation_drifts) / translation_scale
             )
         return (
-            reprojection_cost + geometry_cost,
+            reprojection_cost + geometry_cost + normal_alignment_cost,
             max(rotation_drifts, default=0.0),
             (
                 max(distance_drifts, default=0.0)
@@ -384,9 +419,25 @@ def select_rigid_group_trajectory(
         translation_drifts_by_frame, dtype=float
     )
     distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float)
+    normal_alignments = np.asarray(
+        [
+            max(
+                (
+                    _normal_alignment_rad(frame[first], frame[second])
+                    for first, second in normal_pair_names
+                ),
+                default=0.0,
+            )
+            for frame in best_path
+        ],
+        dtype=float,
+    )
     return best_path, {
         "total_cost": float(best_total),
         "pair_geometry": geometry_mode,
+        "maximum_normal_alignment_rad": float(
+            np.max(normal_alignments, initial=0.0)
+        ),
         "maximum_pair_rotation_drift_rad": float(
             np.max(rotation_drifts, initial=0.0)
         ),
@@ -414,6 +465,172 @@ def select_rigid_group_trajectory(
     }
 
 
+def select_static_rigid_group_initialization(
+    frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
+    *,
+    roles: Sequence[str],
+    fixed_pairs: Sequence[tuple[str, str]],
+    reprojection_scale_px: float,
+    maximum_pose_jump_rad: float,
+    maximum_translation_jump_m: float,
+    relative_rotation_scale_rad: float,
+    relative_translation_scale_m: float,
+    normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+    normal_alignment_scale_rad: float = math.radians(5.0),
+) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
+    """Select a static multi-Tag IPPE branch path in bounded time.
+
+    Group initialization is performed while the hand is held at an endpoint,
+    so every frame should describe the same camera and relative Tag poses.
+    Enumerating a full Viterbi transition matrix for every possible first-frame
+    branch is therefore unnecessary: with four two-branch Tags and eight
+    frames it performs more than one hundred thousand scipy rotations and can
+    block the live ROS callback for several seconds.
+
+    Instead, treat every first-frame combination as a possible static
+    reference and independently select the closest combination in each later
+    frame.  This preserves the multi-frame rigidity and normal-alignment
+    evidence while changing the search from O(F*C^2*C0) to O(F*C*C0).
+    """
+    role_names = tuple(str(role) for role in roles)
+    pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    normal_pair_names = tuple(
+        (str(first), str(second)) for first, second in normal_alignment_pairs
+    )
+    if not frames:
+        raise ValueError("at least one PnP frame is required")
+    if not role_names or len(set(role_names)) != len(role_names):
+        raise ValueError("roles must be non-empty and unique")
+    if any(
+        parent not in role_names or child not in role_names
+        for parent, child in (*pair_names, *normal_pair_names)
+    ):
+        raise ValueError("geometry pairs must reference roles")
+    scales = (
+        float(reprojection_scale_px),
+        float(maximum_pose_jump_rad),
+        float(maximum_translation_jump_m),
+        float(relative_rotation_scale_rad),
+        float(relative_translation_scale_m),
+        float(normal_alignment_scale_rad),
+    )
+    if min(scales) <= 0.0:
+        raise ValueError("static initialization scales must be positive")
+    (
+        reprojection_scale,
+        pose_scale,
+        translation_scale,
+        relative_rotation_scale,
+        relative_translation_scale,
+        normal_scale,
+    ) = scales
+
+    combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
+    for frame in frames:
+        candidate_lists = [tuple(frame.get(role, ())) for role in role_names]
+        if any(not candidates for candidates in candidate_lists):
+            raise ValueError("every frame must contain every requested role")
+        combinations_by_frame.append(
+            [
+                dict(zip(role_names, combination))
+                for combination in product(*candidate_lists)
+            ]
+        )
+
+    def score_against_reference(
+        reference: Mapping[str, SquareTagPose],
+        reference_pairs: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ],
+        combination: Mapping[str, SquareTagPose],
+    ) -> float:
+        score = sum(
+            pose.reprojection_error_px for pose in combination.values()
+        ) / reprojection_scale
+        score += sum(
+            _normal_alignment_rad(combination[first], combination[second])
+            for first, second in normal_pair_names
+        ) / normal_scale
+        score += sum(
+            rotation_distance_rad(
+                reference[role].quaternion_xyzw,
+                combination[role].quaternion_xyzw,
+            )
+            / pose_scale
+            + float(
+                np.linalg.norm(
+                    np.asarray(combination[role].translation_xyz_m, dtype=float)
+                    - np.asarray(reference[role].translation_xyz_m, dtype=float)
+                )
+            )
+            / translation_scale
+            for role in role_names
+        )
+        for pair, (reference_rotation, reference_translation) in (
+            reference_pairs.items()
+        ):
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            score += (
+                float((reference_rotation.inv() * rotation).magnitude())
+                / relative_rotation_scale
+                + float(np.linalg.norm(translation - reference_translation))
+                / relative_translation_scale
+            )
+        return float(score)
+
+    best_total = float("inf")
+    best_path: list[dict[str, SquareTagPose]] | None = None
+    for reference in combinations_by_frame[0]:
+        reference_pairs = {
+            pair: _relative_pose(reference[pair[0]], reference[pair[1]])
+            for pair in pair_names
+        }
+        path = [reference]
+        total = score_against_reference(reference, reference_pairs, reference)
+        for combinations in combinations_by_frame[1:]:
+            scored = [
+                (
+                    score_against_reference(
+                        reference, reference_pairs, combination
+                    ),
+                    combination,
+                )
+                for combination in combinations
+            ]
+            cost, selected = min(scored, key=lambda item: item[0])
+            total += cost
+            path.append(selected)
+        if total < best_total:
+            best_total = total
+            best_path = path
+
+    if best_path is None:
+        raise RuntimeError("static group initialization produced no path")
+
+    # Reuse the complete quality calculation with exactly one chosen branch
+    # per role and frame.  This retains all existing quality fields without
+    # reintroducing the combinatorial branch search.
+    reduced_frames = [
+        {role: (frame[role],) for role in role_names} for frame in best_path
+    ]
+    selected_path, quality = select_rigid_group_trajectory(
+        reduced_frames,
+        roles=role_names,
+        fixed_pairs=pair_names,
+        reprojection_scale_px=reprojection_scale,
+        rotation_scale_rad=relative_rotation_scale,
+        translation_scale_m=relative_translation_scale,
+        normal_alignment_pairs=normal_pair_names,
+        normal_alignment_scale_rad=normal_scale,
+    )
+    quality = dict(quality)
+    quality["total_cost"] = float(best_total)
+    quality["initialization_search"] = "static_reference"
+    return selected_path, quality
+
+
 def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray:
     matrix = np.asarray(camera_matrix, dtype=np.float64)
     if matrix.shape != (3, 3):
@@ -517,9 +734,26 @@ def rotation_distance_rad(
     first_xyzw: Sequence[float],
     second_xyzw: Sequence[float],
 ) -> float:
-    first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float))
-    second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float))
-    return float((first.inv() * second).magnitude())
+    first = np.asarray(first_xyzw, dtype=float)
+    second = np.asarray(second_xyzw, dtype=float)
+    if first.shape != (4,) or second.shape != (4,):
+        raise ValueError("quaternions must contain four values")
+    first_norm = float(np.linalg.norm(first))
+    second_norm = float(np.linalg.norm(second))
+    if (
+        not np.all(np.isfinite(first))
+        or not np.all(np.isfinite(second))
+        or first_norm <= 0.0
+        or second_norm <= 0.0
+    ):
+        raise ValueError("quaternions must be finite and non-zero")
+    # q and -q represent the same rotation.  The absolute dot-product gives
+    # the geodesic SO(3) distance without constructing two scipy Rotation
+    # objects for every branch comparison in the live tracker.
+    cosine_half_angle = abs(
+        float(np.dot(first / first_norm, second / second_norm))
+    )
+    return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0)))
 
 
 def select_continuous_pose(
@@ -775,19 +1009,30 @@ class SquareTagGroupPoseTracker:
         reprojection_scale_px: float,
         reprojection_weight: float,
         reset_after_seconds: float,
+        initialization_frames: int = 1,
+        normal_alignment_pairs: Sequence[tuple[str, str]] = (),
+        normal_alignment_scale_rad: float = math.radians(5.0),
+        maximum_normal_alignment_rad: float | None = None,
     ) -> None:
         self.roles = tuple(str(role) for role in roles)
         self.adjacent_pairs = tuple(
             (str(parent), str(child))
             for parent, child in adjacent_pairs
         )
+        self.normal_alignment_pairs = tuple(
+            (str(first), str(second))
+            for first, second in normal_alignment_pairs
+        )
         if not self.roles or len(set(self.roles)) != len(self.roles):
             raise ValueError("roles must be non-empty and unique")
         if any(
             parent not in self.roles or child not in self.roles
-            for parent, child in self.adjacent_pairs
+            for parent, child in (
+                *self.adjacent_pairs,
+                *self.normal_alignment_pairs,
+            )
         ):
-            raise ValueError("adjacent_pairs must reference roles")
+            raise ValueError("group geometry pairs must reference roles")
         self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
         self.maximum_translation_jump_m = float(
             maximum_translation_jump_m
@@ -800,6 +1045,15 @@ class SquareTagGroupPoseTracker:
         )
         self.reprojection_scale_px = float(reprojection_scale_px)
         self.reprojection_weight = float(reprojection_weight)
+        self.initialization_frames = int(initialization_frames)
+        self.normal_alignment_scale_rad = float(
+            normal_alignment_scale_rad
+        )
+        self.maximum_normal_alignment_rad = (
+            None
+            if maximum_normal_alignment_rad is None
+            else float(maximum_normal_alignment_rad)
+        )
         reset_seconds = float(reset_after_seconds)
         if min(
             self.maximum_pose_jump_rad,
@@ -807,19 +1061,35 @@ class SquareTagGroupPoseTracker:
             self.relative_rotation_scale_rad,
             self.relative_translation_scale_m,
             self.reprojection_scale_px,
+            self.normal_alignment_scale_rad,
             reset_seconds,
         ) <= 0.0:
             raise ValueError("group tracking scales must be positive")
         if self.reprojection_weight < 0.0:
             raise ValueError("reprojection_weight must be non-negative")
+        if self.initialization_frames < 1:
+            raise ValueError("initialization_frames must be positive")
+        if (
+            self.maximum_normal_alignment_rad is not None
+            and self.maximum_normal_alignment_rad <= 0.0
+        ):
+            raise ValueError("maximum normal alignment must be positive")
         self.reset_after_ns = int(reset_seconds * 1_000_000_000)
         self._previous: dict[str, SquareTagPose] = {}
         self._previous_stamp_ns: int | None = None
+        self._initial_candidates: list[
+            dict[str, tuple[SquareTagPose, ...]]
+        ] = []
+        self._initial_stamps_ns: list[int] = []
+        self.last_initialization_quality: dict[str, float | str] = {}
         self.branch_correction_counts: dict[str, int] = {}
 
     def reset(self) -> None:
         self._previous.clear()
         self._previous_stamp_ns = None
+        self._initial_candidates.clear()
+        self._initial_stamps_ns.clear()
+        self.last_initialization_quality.clear()
         self.branch_correction_counts.clear()
 
     def select(
@@ -856,13 +1126,110 @@ class SquareTagGroupPoseTracker:
         )
 
         if not previous_is_fresh:
-            selected = min(
-                combinations,
-                key=lambda combination: sum(
-                    pose.reprojection_error_px
-                    for pose in combination.values()
-                ),
-            )
+            if self._previous_stamp_ns is not None:
+                self._previous.clear()
+                self._previous_stamp_ns = None
+                self._initial_candidates.clear()
+                self._initial_stamps_ns.clear()
+                self.last_initialization_quality.clear()
+            if self.initialization_frames > 1:
+                if self._initial_stamps_ns and not (
+                    0 <= stamp - self._initial_stamps_ns[-1]
+                    <= self.reset_after_ns
+                ):
+                    self._initial_candidates.clear()
+                    self._initial_stamps_ns.clear()
+                self._initial_candidates.append(
+                    {
+                        role: tuple(candidates_by_role.get(role, ()))
+                        for role in self.roles
+                    }
+                )
+                self._initial_stamps_ns.append(stamp)
+                if len(self._initial_candidates) < self.initialization_frames:
+                    return (
+                        None,
+                        "group_initializing:"
+                        f"{len(self._initial_candidates)}/"
+                        f"{self.initialization_frames}",
+                    )
+                selected_path, initialization_quality = (
+                    select_static_rigid_group_initialization(
+                        self._initial_candidates,
+                        roles=self.roles,
+                        fixed_pairs=self.adjacent_pairs,
+                        reprojection_scale_px=self.reprojection_scale_px,
+                        maximum_pose_jump_rad=self.maximum_pose_jump_rad,
+                        maximum_translation_jump_m=(
+                            self.maximum_translation_jump_m
+                        ),
+                        relative_rotation_scale_rad=(
+                            self.relative_rotation_scale_rad
+                        ),
+                        relative_translation_scale_m=(
+                            self.relative_translation_scale_m
+                        ),
+                        normal_alignment_pairs=(
+                            self.normal_alignment_pairs
+                        ),
+                        normal_alignment_scale_rad=(
+                            self.normal_alignment_scale_rad
+                        ),
+                    )
+                )
+                selected = selected_path[-1]
+                stamp = self._initial_stamps_ns[-1]
+                self.last_initialization_quality = dict(
+                    initialization_quality
+                )
+                self._initial_candidates.clear()
+                self._initial_stamps_ns.clear()
+                if (
+                    self.maximum_normal_alignment_rad is not None
+                    and float(
+                        initialization_quality[
+                            "maximum_normal_alignment_rad"
+                        ]
+                    )
+                    > self.maximum_normal_alignment_rad
+                ):
+                    return None, "group_normal_alignment"
+            else:
+                selected = min(
+                    combinations,
+                    key=lambda combination: (
+                        sum(
+                            pose.reprojection_error_px
+                            for pose in combination.values()
+                        )
+                        / self.reprojection_scale_px
+                        + sum(
+                            _normal_alignment_rad(
+                                combination[first], combination[second]
+                            )
+                            for first, second in self.normal_alignment_pairs
+                        )
+                        / self.normal_alignment_scale_rad
+                    ),
+                )
+                maximum_alignment = max(
+                    (
+                        _normal_alignment_rad(
+                            selected[first], selected[second]
+                        )
+                        for first, second in self.normal_alignment_pairs
+                    ),
+                    default=0.0,
+                )
+                self.last_initialization_quality = {
+                    "maximum_normal_alignment_rad": maximum_alignment
+                }
+                if (
+                    self.maximum_normal_alignment_rad is not None
+                    and maximum_alignment
+                    > self.maximum_normal_alignment_rad
+                ):
+                    return None, "group_normal_alignment"
         else:
             previous_relative = {
                 pair: _relative_pose(

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py

@@ -9,7 +9,7 @@ from typing import Any, Mapping
 STATE_NAMES_ZH = {
     "PREFLIGHT": "设备和标签预检",
     "WAIT_START": "等待开始标定",
-    "RETURN_BASELINE": "正在返回基准姿态",
+    "RETURN_BASELINE": "正在恢复目标姿态",
     "PREPARE_SWEEP": "正在到达扫描起点",
     "SWEEP": "正在采集轨迹",
     "FITTING": "正在拟合轨迹和零位",
@@ -35,6 +35,18 @@ JOINT_NAMES_ZH = {
     "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侧摆",
 }
 
@@ -59,6 +71,21 @@ def _task_text(active: Mapping[str, Any]) -> str:
             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')}，"
@@ -72,10 +99,17 @@ def _task_text(active: Mapping[str, Any]) -> str:
     target = active.get("target_u8")
     cycle = active.get("cycle", "?")
     repetitions = active.get("repetitions", "?")
-    return (
+    task = (
         f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
         f"第{cycle}/{repetitions}轮，{start}→{target}"
     )
+    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(
@@ -93,6 +127,29 @@ def three_camera_reason_zh(
     )
     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
@@ -249,6 +306,45 @@ def three_camera_reason_zh(
             "/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同时就绪。",
@@ -274,7 +370,7 @@ def three_camera_reason_zh(
     if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
         return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
     if state == "RETURN_BASELINE":
-        return "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。"
+        return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
     if state == "FITTING":
         return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
     return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
@@ -290,18 +386,38 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
     progress = float(payload.get("progress", 0.0))
     completed = payload.get("completed_sweeps", 0)
     total = payload.get("total_sweeps", 0)
+    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"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
+        f"扫描进度{scan_progress:.1%}）",
         f"当前任务：{_task_text(active)}",
     ]
     if state == "RETURN_BASELINE":
-        lines.append(
-            f"正在确认基准姿态：{payload.get('baseline_command_u8', [])}"
-        )
-    if active and active.get("kind") != "fit_failure":
+        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 = (
@@ -333,6 +449,16 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
                 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", [])

src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py

@@ -0,0 +1,37 @@
+"""Publish calibrated G20 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_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=[
+                    "g20_calibrated_joint_state_bridge_",
+                    LaunchConfiguration("hand_type"),
+                ],
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "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/three_camera_calibration.launch.py

@@ -25,7 +25,34 @@ from launch_ros.parameter_descriptions import ParameterValue
 VIEWS = ("front", "side", "top")
 
 
+def _default_source_urdf(hand_type: str) -> Path:
+    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_type = LaunchConfiguration("hand_type").perform(context).lower()
+    if hand_type not in {"left", "right"}:
+        raise RuntimeError("hand_type must be left or right")
+    command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
+    state_topic = f"/g20/cb_{hand_type}_hand_state"
+    info_topic = f"/g20/cb_{hand_type}_hand_info"
+    requested_source = LaunchConfiguration("source_urdf_path").perform(context)
+    source_urdf = (
+        Path(requested_source).expanduser().resolve()
+        if requested_source
+        else _default_source_urdf(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
@@ -170,7 +197,7 @@ def _launch_stack(context):
         condition=IfCondition(LaunchConfiguration("start_sdk")),
         parameters=[
             {
-                "hand_type": "left",
+                "hand_type": hand_type,
                 "hand_joint": "G20",
                 "can": LaunchConfiguration("can_interface"),
                 "modbus": "None",
@@ -182,6 +209,15 @@ def _launch_stack(context):
                 "startup_torque": 80,
                 # Match 30 Hz cameras so state/image p95 skew stays below 50 ms.
                 "state_poll_rate": 30.0,
+                # Calibration does not consume measured joint velocity.  A
+                # G20 velocity read sends another five synchronous CAN
+                # queries, so keep it off the trajectory-critical path.
+                "velocity_poll_rate": 1.0,
+                # Calibration sends one endpoint command per sweep.  Keep
+                # polling the real motor state during the following motion;
+                # otherwise the SDK republishes a stale state for 0.2 s and
+                # creates 17-27 command-unit holes in the trajectory bins.
+                "defer_state_reads_while_commanding": False,
                 "repeat_position_commands": False,
                 "is_touch": False,
             }
@@ -197,11 +233,19 @@ def _launch_stack(context):
             LaunchConfiguration("calibration_config"),
             {
                 "serial_number": hand_serial,
+                "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": "/g20_calibration/disabled_hand_info",
+                "command_topic": command_topic,
+                "state_topic": state_topic,
                 "camera_extrinsics_file": LaunchConfiguration(
                     "camera_extrinsics_file"
                 ),
-                "source_urdf_path": LaunchConfiguration("source_urdf_path"),
+                "source_urdf_path": str(source_urdf),
                 "corrected_urdf_output_dir": LaunchConfiguration(
                     "corrected_urdf_output_dir"
                 ),
@@ -246,15 +290,20 @@ def _launch_stack(context):
             *raw_topics,
             *info_topics,
             *detection_topics,
-            "/g20/cb_left_hand_control_cmd",
-            "/g20/cb_left_hand_state",
-            "/g20/cb_left_hand_info",
+            command_topic,
+            state_topic,
+            info_topic,
             "/g20_calibration/status",
         ],
         output="screen",
     )
     return [
-        LogInfo(msg=f"G20 three-camera session: {session_dir}"),
+        LogInfo(
+            msg=(
+                f"G20 {hand_type} three-camera session: {session_dir}; "
+                f"source_urdf={source_urdf}"
+            )
+        ),
         LogInfo(
             msg=(
                 "Camera mapping: front="
@@ -275,31 +324,24 @@ def generate_launch_description() -> LaunchDescription:
         get_package_share_directory("g20_thumb_apriltag_calibration")
     )
     info_root = Path.home() / ".ros" / "camera_info"
-    workspace_source_urdf = (
-        Path.cwd()
-        / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
-        "linker_hand/g20_left/linkerhand_g20_left.urdf"
-    )
-    try:
-        linkerhand_share = Path(
-            get_package_share_directory("linkerhand_retarget")
-        )
-        installed_source_urdf = linkerhand_share / (
-            "assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf"
-        )
-    except Exception:
-        installed_source_urdf = workspace_source_urdf
-    default_source_urdf = (
-        workspace_source_urdf
-        if workspace_source_urdf.is_file()
-        else installed_source_urdf
-    )
     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_type", default_value="left"),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
             DeclareLaunchArgument(
                 "front_camera_serial", default_value="DB2163742"
@@ -353,7 +395,7 @@ def generate_launch_description() -> LaunchDescription:
                 ),
             ),
             DeclareLaunchArgument(
-                "source_urdf_path", default_value=str(default_source_urdf)
+                "source_urdf_path", default_value=""
             ),
             DeclareLaunchArgument(
                 "corrected_urdf_output_dir", default_value=""

src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py

@@ -112,6 +112,12 @@ def _launch(context):
                     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,
@@ -137,6 +143,18 @@ def generate_launch_description() -> LaunchDescription:
     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(
@@ -164,6 +182,10 @@ def generate_launch_description() -> LaunchDescription:
             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"

src/g20_thumb_apriltag_calibration/setup.py

@@ -51,10 +51,19 @@ setup(
                 "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_calibrated_joint_state_bridge.py

@@ -0,0 +1,87 @@
+import copy
+
+import pytest
+
+from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
+    G20_COMMAND_NAMES,
+    G20_URDF_JOINT_NAMES,
+    CalibratedCommandMapper,
+)
+from g20_thumb_apriltag_calibration.full_hand import (
+    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"])

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -25,6 +25,18 @@ def test_fastdds_profile_has_capacity_for_full_resolution_images() -> 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()
@@ -43,7 +55,7 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
     assert detector["detector"]["debug"] is False
 
 
-def test_three_camera_tag_ids_and_topics_are_disjoint() -> None:
+def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
     tags = yaml.safe_load(
         (PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text()
     )
@@ -63,6 +75,9 @@ def test_three_camera_tag_ids_and_topics_are_disjoint() -> None:
         assert parameters["detector"]["decimate"] == 1.5
         all_ids.update(ids)
     assert all_ids == set(range(11))
+    assert tags[
+        "/g20_calibration/side/apriltag/apriltag"
+    ]["ros__parameters"]["tag"]["frames"][0] == "side_base"
 
 
 def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
@@ -99,25 +114,41 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
     assert parameters["normal_calibration_speed"] == 15
     assert parameters["index_roll_calibration_speed"] == 5
     assert parameters["index_flex_calibration_speed"] == 10
+    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["thumb_yaw_zero_endpoint_tolerance_u8"] == 3.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

src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py

@@ -4,11 +4,14 @@ 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,
 )
@@ -100,6 +103,135 @@ def test_repeatability_summary_exposes_current_capture_outlier():
     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)

src/g20_thumb_apriltag_calibration/test/test_full_hand.py

@@ -8,8 +8,10 @@ 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,
@@ -22,6 +24,7 @@ from g20_thumb_apriltag_calibration.full_hand import (
     fit_joint_image_curve,
     fit_measured_joint_curve,
     fit_projected_zero,
+    get_hand_calibration_profile,
     measure_joint_observation,
     validate_compact_payload,
 )
@@ -70,6 +73,55 @@ 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)
     )
@@ -122,6 +174,20 @@ def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
     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]
@@ -133,6 +199,33 @@ def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
     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]
@@ -342,3 +435,66 @@ def test_compact_payload_allows_skipped_random_validation() -> None:
         "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_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

@@ -324,6 +324,125 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
     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 = {

src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py

@@ -3,7 +3,9 @@ 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,
 )
 
 
@@ -64,7 +66,7 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
     assert "缺少电机端点255" in text
     assert "实际电机范围为0.4～248.2" in text
     assert "第1/3轮，255→0" in text
-    assert "已完成6/42个扫描方向" 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
@@ -135,6 +137,107 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
     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_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]
@@ -152,10 +255,34 @@ def test_return_baseline_prints_the_exact_command() -> None:
 
     text = render_three_camera_status_text_zh(payload)
 
-    assert "正在返回基准姿态（RETURN_BASELINE）" in text
+    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 = {
@@ -214,6 +341,8 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
             "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,
@@ -241,3 +370,4 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
     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_three_camera_retry.py

@@ -10,13 +10,17 @@ from g20_thumb_apriltag_calibration.core import (
     DIRECTION_DECREASING,
     DIRECTION_INCREASING,
 )
-from g20_thumb_apriltag_calibration.full_hand import SWEEP_SPECS
+from g20_thumb_apriltag_calibration.full_hand import (
+    RIGHT_HAND_PROFILE,
+    SWEEP_SPECS,
+)
 from g20_thumb_apriltag_calibration.three_camera_node import (
     FrameObservation,
     G20ThreeCameraCalibrationNode,
     STATE_PREPARE_SWEEP,
     STATE_SWEEP,
     SweepItem,
+    _overall_progress,
 )
 
 
@@ -45,6 +49,78 @@ def _frame(view: str, state_u8: list[float]) -> FrameObservation:
     )
 
 
+def test_right_thumb_pitch_requires_only_front_base_and_thumb_tag() -> None:
+    profile = RIGHT_HAND_PROFILE
+    spec = next(item for item in profile.sweep_specs if item.motor_index == 0)
+    runtime = SimpleNamespace(
+        roles=tuple(profile.view_tags["front"]),
+        preflight_roles=profile.preflight_view_roles["front"],
+    )
+    node = SimpleNamespace(
+        profile=profile,
+        views={"front": runtime},
+        active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING),
+        active_validation=None,
+        retry_sweep_spec=None,
+    )
+
+    required = G20ThreeCameraCalibrationNode._required_roles_for_view(
+        node, "front"
+    )
+
+    assert required == ("front_base", "thumb_cmc")
+
+
+def test_right_side_preflight_requires_only_pinky_chain_tags() -> None:
+    profile = RIGHT_HAND_PROFILE
+    runtime = SimpleNamespace(
+        roles=tuple(profile.view_tags["side"]),
+        preflight_roles=profile.preflight_view_roles["side"],
+    )
+    node = SimpleNamespace(
+        profile=profile,
+        views={"side": runtime},
+        active_sweep=None,
+        active_validation=None,
+        retry_sweep_spec=None,
+    )
+
+    required = G20ThreeCameraCalibrationNode._required_roles_for_view(
+        node, "side"
+    )
+
+    assert required == ("side_base", "pinky_mcp", "pinky_pip", "pinky_dip")
+
+
+def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None:
+    profile = RIGHT_HAND_PROFILE
+    spec = next(
+        item for item in profile.sweep_specs if item.motor_index == 19
+    )
+    runtime = SimpleNamespace(
+        roles=tuple(profile.view_tags["side"]),
+        preflight_roles=profile.preflight_view_roles["side"],
+    )
+    node = SimpleNamespace(
+        profile=profile,
+        views={"side": runtime},
+        active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING),
+        active_validation=None,
+        retry_sweep_spec=None,
+    )
+
+    required = G20ThreeCameraCalibrationNode._required_roles_for_view(
+        node, "side"
+    )
+
+    assert required == (
+        "side_base",
+        "pinky_mcp",
+        "pinky_pip",
+        "pinky_dip",
+    )
+
+
 def test_prepare_sweep_retains_synchronised_start_endpoint_frame() -> None:
     spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
     item = SweepItem(spec, 2, DIRECTION_DECREASING)
@@ -98,6 +174,8 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None:
         sweep_frames=[],
         sweep_start_frames=[endpoint_frame],
         _publish_command=lambda command: published.append(command),
+        _motion_command_error_u8=lambda selected, command: 255.0,
+        _reset_motion_progress=lambda now, error: None,
     )
 
     G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0)
@@ -110,35 +188,124 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None:
     assert published[0][7:10] == [0, 0, 0]
 
 
-def test_only_thumb_yaw_uses_three_count_endpoint_deadband() -> None:
+def test_joint_specific_zero_endpoint_deadbands() -> None:
     yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10)
     index_roll = next(item for item in SWEEP_SPECS if item.motor_index == 6)
+    pinky_pip = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 19
+    )
     node = SimpleNamespace(
         endpoint_tolerance_u8=2.0,
-        thumb_yaw_zero_endpoint_tolerance_u8=3.0,
+        thumb_yaw_zero_endpoint_tolerance_u8=4.0,
+        right_thumb_yaw_255_endpoint_tolerance_u8=5.0,
+        pinky_pip_zero_endpoint_tolerance_u8=5.0,
     )
 
     assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
         node, yaw, 0
-    ) == 3.0
+    ) == 4.0
     assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
         node, yaw, 255
     ) == 2.0
+    node.profile = RIGHT_HAND_PROFILE
+    assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
+        node, yaw, 255
+    ) == 5.0
     assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
         node, index_roll, 0
     ) == 2.0
+    assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
+        node, pinky_pip, 0
+    ) == 5.0
+    assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
+        node, pinky_pip, 255
+    ) == 2.0
 
 
-def test_thumb_yaw_command_zero_accepts_feedback_three_only_for_swept_motor() -> None:
+def test_pinky_pip_command_zero_accepts_firmware_feedback_five() -> None:
+    pinky_pip = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 19
+    )
+    state = [255.0] * 20
+    state[19] = 5.0
+    node = SimpleNamespace(
+        latest_state_u8=tuple(state),
+        baseline_command=tuple([255] * 20),
+        endpoint_tolerance_u8=2.0,
+        pinky_pip_zero_endpoint_tolerance_u8=5.0,
+    )
+    node._endpoint_tolerance_for_spec = lambda spec, endpoint: (
+        G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
+            node, spec, endpoint
+        )
+    )
+
+    assert G20ThreeCameraCalibrationNode._motion_command_reached(
+        node, pinky_pip, 0, tuple(state)
+    )
+    state[19] = 6.0
+    assert not G20ThreeCameraCalibrationNode._motion_command_reached(
+        node, pinky_pip, 0, tuple(state)
+    )
+
+
+def test_right_baseline_and_auxiliary_accept_thumb_yaw_feedback_250() -> None:
+    state = [253.0] * 20
+    state[0] = 254.0
+    state[5] = 254.0
+    state[6:10] = [127.0] * 4
+    state[10] = 250.0
+    state[15] = 255.0
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    node = SimpleNamespace(
+        profile=RIGHT_HAND_PROFILE,
+        latest_state_u8=tuple(state),
+        baseline_command=tuple(baseline),
+        endpoint_tolerance_u8=2.0,
+        thumb_yaw_zero_endpoint_tolerance_u8=4.0,
+        right_thumb_yaw_255_endpoint_tolerance_u8=5.0,
+        pinky_pip_zero_endpoint_tolerance_u8=5.0,
+    )
+    node._motor_endpoint_tolerance = lambda motor, endpoint: (
+        G20ThreeCameraCalibrationNode._motor_endpoint_tolerance(
+            node, motor, endpoint
+        )
+    )
+    node._endpoint_tolerance_for_spec = lambda spec, endpoint: (
+        G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
+            node, spec, endpoint
+        )
+    )
+
+    assert G20ThreeCameraCalibrationNode._baseline_reached(node)
+    details = G20ThreeCameraCalibrationNode._baseline_error_details(node)
+    assert details["motor_index"] == 10
+    assert details["actual_u8"] == 250.0
+    assert details["tolerance_u8"] == 5.0
+
+    thumb_pitch = next(
+        spec for spec in RIGHT_HAND_PROFILE.sweep_specs
+        if spec.motor_index == 0
+    )
+    state[0] = 255.0
+    assert G20ThreeCameraCalibrationNode._motion_command_reached(
+        node, thumb_pitch, 255, tuple(state)
+    )
+
+
+def test_thumb_yaw_command_zero_accepts_feedback_four_only_for_swept_motor() -> None:
     yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10)
     state = [255.0] * 20
-    state[10] = 3.0
+    state[10] = 4.0
     state[5] = 143.0
     node = SimpleNamespace(
         latest_state_u8=tuple(state),
         baseline_command=tuple([255] * 20),
         endpoint_tolerance_u8=2.0,
-        thumb_yaw_zero_endpoint_tolerance_u8=3.0,
+        thumb_yaw_zero_endpoint_tolerance_u8=4.0,
     )
     node._endpoint_tolerance_for_spec = lambda spec, endpoint: (
         G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
@@ -149,12 +316,128 @@ def test_thumb_yaw_command_zero_accepts_feedback_three_only_for_swept_motor() ->
     assert G20ThreeCameraCalibrationNode._motion_command_reached(
         node, yaw, 0, tuple(state)
     )
+    state[10] = 5.0
+    assert not G20ThreeCameraCalibrationNode._motion_command_reached(
+        node, yaw, 0, tuple(state)
+    )
+    state[10] = 4.0
     state[5] = 142.0
     assert not G20ThreeCameraCalibrationNode._motion_command_reached(
         node, yaw, 0, tuple(state)
     )
 
 
+def test_thumb_yaw_direction_recovery_keeps_motor_five_at_clearance() -> None:
+    yaw = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 10
+    )
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    for direction, expected_start in (
+        (DIRECTION_DECREASING, 255),
+        (DIRECTION_INCREASING, 0),
+    ):
+        node = SimpleNamespace(
+            profile=RIGHT_HAND_PROFILE,
+            baseline_command=tuple(baseline),
+            active_sweep=SweepItem(yaw, 1, direction),
+            retry_sweep_items=[],
+        )
+
+        command = G20ThreeCameraCalibrationNode._return_command_for_transition(
+            node, "retry_sweep"
+        )
+
+        assert command[5] == 145
+        assert command[10] == expected_start
+        assert all(
+            command[index] == baseline[index]
+            for index in range(20)
+            if index not in {5, 10}
+        )
+
+
+def test_thumb_yaw_cycle_and_manual_recovery_use_queued_direction_start() -> None:
+    yaw = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 10
+    )
+    other = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index != 10
+    )
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    queued = SweepItem(yaw, 2, DIRECTION_INCREASING)
+    node = SimpleNamespace(
+        profile=RIGHT_HAND_PROFILE,
+        baseline_command=tuple(baseline),
+        active_sweep=SweepItem(other, 0, DIRECTION_DECREASING),
+        retry_sweep_items=[queued],
+    )
+
+    command = G20ThreeCameraCalibrationNode._return_command_for_transition(
+        node, "resume_sweep"
+    )
+
+    assert command[5] == 145
+    assert command[10] == 0
+
+
+def test_thumb_yaw_clearance_is_released_only_for_normal_transition() -> None:
+    yaw = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 10
+    )
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    node = SimpleNamespace(
+        profile=RIGHT_HAND_PROFILE,
+        baseline_command=tuple(baseline),
+        active_sweep=SweepItem(yaw, 2, DIRECTION_INCREASING),
+        retry_sweep_items=[],
+    )
+
+    command = G20ThreeCameraCalibrationNode._return_command_for_transition(
+        node, "fit"
+    )
+
+    assert command == tuple(baseline)
+    assert command[5] == 255
+
+
+def test_thumb_yaw_recovery_zero_accepts_feedback_four() -> None:
+    yaw = next(
+        item for item in RIGHT_HAND_PROFILE.sweep_specs
+        if item.motor_index == 10
+    )
+    baseline = [255] * 20
+    baseline[6:10] = [127] * 4
+    return_command = list(baseline)
+    return_command[5] = 145
+    return_command[10] = 0
+    state = [float(value) for value in return_command]
+    state[10] = 4.0
+    node = SimpleNamespace(
+        profile=RIGHT_HAND_PROFILE,
+        baseline_command=tuple(baseline),
+        return_command_u8=tuple(return_command),
+        latest_state_u8=tuple(state),
+        endpoint_tolerance_u8=2.0,
+        thumb_yaw_zero_endpoint_tolerance_u8=4.0,
+        right_thumb_yaw_255_endpoint_tolerance_u8=5.0,
+    )
+
+    assert G20ThreeCameraCalibrationNode._baseline_reached(node)
+    details = G20ThreeCameraCalibrationNode._motion_command_error_details(
+        node, yaw, 0, "return_baseline"
+    )
+    assert details["motor_index"] == 10
+    assert details["actual_u8"] == 4.0
+    assert details["tolerance_u8"] == 4.0
+
+
 def _image_cycle_records(
     travels_rad: list[float], *, depth_slope: float = 0.0
 ) -> list[dict]:
@@ -225,6 +508,7 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace:
         axis_maximum_plane_rms_m=0.003,
         passive_axis_maximum_plane_rms_m=0.004,
         axis_maximum_radial_rms_m=0.003,
+        axis_maximum_pose_line_rms_m=0.001,
         axis_maximum_rotation_circle_difference_rad=math.radians(1.0),
         active_maximum_rotation_orthogonal_rms_rad=math.radians(2.5),
         passive_maximum_rotation_orthogonal_rms_rad=math.radians(7.5),
@@ -241,7 +525,7 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace:
     return node
 
 
-def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None:
+def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None:
     spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
     node = SimpleNamespace(
         sweep_items=_sweep_items(),
@@ -272,17 +556,97 @@ def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None:
         ],
     )
 
-    assert node.sweep_index == 18
+    assert node.sweep_index == 24
     assert node.retry_sweep_spec == spec
+    assert node.retry_cycles == {0, 1, 2}
     assert node.fit_failure["directions_to_rescan"] == 6
     assert node.paused_reason == "joint_fit_check_failed"
 
 
+def test_zero_model_failure_never_schedules_an_automatic_rescan(tmp_path) -> None:
+    pauses: list[str] = []
+    node = SimpleNamespace(
+        profile=RIGHT_HAND_PROFILE,
+        raw_path=tmp_path / "raw_samples.jsonl",
+        maximum_validation_p95_rad=math.radians(2.0),
+        zero_maximum_offset_rad=math.radians(20.0),
+        zero_joint_maximum_offsets_rad={},
+        retry_sweep_spec=RIGHT_HAND_PROFILE.sweep_specs[2],
+        retry_resume_index=42,
+        retry_cycles={0, 1, 2},
+        fit_failure={},
+        _pause=lambda reason: pauses.append(reason),
+    )
+    result = SimpleNamespace(
+        failure_reasons={
+            "thumb_mcp": "zero_offset_exceeds_configured_limit"
+        },
+        direct_offsets_rad={"thumb_mcp": math.radians(-39.81)},
+        validation_error_by_joint_rad={
+            "thumb_ip": math.radians(23.5)
+        },
+    )
+
+    G20ThreeCameraCalibrationNode._pause_for_zero_model_failure(node, result)
+
+    assert pauses == ["zero_model_validation_failed"]
+    assert node.retry_sweep_spec is None
+    assert node.retry_resume_index is None
+    assert node.retry_cycles == set()
+    assert node.fit_failure["recoverable_by_rescan"] is False
+    assert node.fit_failure["directions_to_rescan"] == 0
+    assert node.fit_failure["failures"][0]["actual_deg"] == -39.81
+    assert node.fit_failure["failures"][0]["limit_deg"] == 20.0
+    event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
+    assert event["kind"] == "zero_model_failure"
+
+
+def test_resume_rejects_zero_model_failure_without_starting_motion() -> None:
+    node = SimpleNamespace(
+        state="PAUSED",
+        paused_reason="zero_model_validation_failed",
+    )
+    response = SimpleNamespace(success=None, message="")
+
+    result = G20ThreeCameraCalibrationNode._resume_callback(
+        node, SimpleNamespace(), response
+    )
+
+    assert result.success is False
+    assert "cannot repair" in result.message
+
+
+def test_scan_completion_is_not_reported_as_calibration_completion() -> None:
+    assert _overall_progress("SWEEP", scan_progress=1.0) == 0.90
+    assert _overall_progress("FITTING", scan_progress=1.0) == 0.92
+    assert np.isclose(
+        _overall_progress(
+            "VALIDATION_CAPTURE",
+            scan_progress=1.0,
+            validation_index=5,
+            validation_total=10,
+        ),
+        0.955,
+    )
+    assert _overall_progress("COMPLETE", scan_progress=1.0) == 1.0
+
+
 def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
     spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
+    reset_runtimes = []
+    front_runtime = SimpleNamespace(
+        pnp_invalid_since=12.0,
+        pnp_reset_count=0,
+    )
     records = {
-        "thumb_mcp": [{"old": "mcp"}],
-        "thumb_ip": [{"old": "ip"}],
+        "thumb_mcp": [
+            {"cycle": 0, "keep": "mcp"},
+            {"cycle": 1, "old": "mcp"},
+        ],
+        "thumb_ip": [
+            {"cycle": 0, "keep": "ip"},
+            {"cycle": 1, "old": "ip"},
+        ],
         "thumb_cmc_pitch": [{"keep": True}],
     }
     node = SimpleNamespace(
@@ -292,17 +656,25 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
         raw_path=tmp_path / "raw_samples.jsonl",
         paused_reason="joint_fit_check_failed",
         fit_failure={"failures": ["old"]},
+        retry_cycles={1},
+        repetitions=3,
+        sweep_items=_sweep_items(),
+        views={"front": front_runtime},
+        _reset_view_trackers=lambda runtime: reset_runtimes.append(runtime),
     )
 
     selected = G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node)
 
     assert selected == spec
-    assert records["thumb_mcp"] == []
-    assert records["thumb_ip"] == []
+    assert records["thumb_mcp"] == [{"cycle": 0, "keep": "mcp"}]
+    assert records["thumb_ip"] == [{"cycle": 0, "keep": "ip"}]
     assert records["thumb_cmc_pitch"] == [{"keep": True}]
     assert node.sweep_attempts[15] == 2
     assert node.retry_sweep_spec == spec
     assert node.fit_failure == {}
+    assert reset_runtimes == [front_runtime]
+    assert front_runtime.pnp_invalid_since is None
+    assert front_runtime.pnp_reset_count == 1
     event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
     assert event == {
         "kind": "retry",
@@ -311,9 +683,195 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
         "joints": ["thumb_mcp", "thumb_ip"],
         "attempt": 2,
         "reason": "joint_fit_check_failed",
+        "cycles": [2],
     }
 
 
+def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> None:
+    spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
+    item = SweepItem(spec, 0, DIRECTION_INCREASING)
+    transitions: list[str] = []
+    pauses: list[str] = []
+    holds: list[bool] = []
+    node = SimpleNamespace(
+        active_sweep=item,
+        automatic_sweep_retry_limit=3,
+        sweep_retry_counts={},
+        raw_path=tmp_path / "raw_samples.jsonl",
+        reason="",
+        _publish_hold_current=lambda: holds.append(True),
+        _begin_return_baseline=lambda after: transitions.append(after),
+        _pause=lambda reason: pauses.append(reason),
+        retry_speed_scales=(0.8, 0.6, 0.5),
+        retry_endpoint_hold_seconds=(0.75, 1.0, 1.25),
+    )
+
+    G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
+        node, "sweep_bin_gap_too_large"
+    )
+    G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
+        node, "sweep_bin_gap_too_large"
+    )
+    G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
+        node, "sweep_bin_gap_too_large"
+    )
+    G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
+        node, "sweep_bin_gap_too_large"
+    )
+
+    assert transitions == ["retry_sweep", "retry_sweep", "retry_sweep"]
+    assert pauses == ["sweep_bin_gap_too_large"]
+    assert holds == []
+    assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 3
+    events = [
+        json.loads(line)
+        for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines()
+    ]
+    assert [event["retry"] for event in events] == [1, 2, 3]
+
+
+def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None:
+    spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
+    transitions: list[str] = []
+    pauses: list[str] = []
+    prepared: list[bool] = []
+    node = SimpleNamespace(
+        sweep_items=_sweep_items(),
+        sweep_attempts={spec.motor_index: 1},
+        retry_sweep_spec=None,
+        retry_resume_index=None,
+        fit_failure={},
+        sweep_index=18,
+        raw_path=tmp_path / "raw_samples.jsonl",
+        repetitions=3,
+        automatic_fit_retry_limit=1,
+        paused_reason="",
+        reason="",
+        _sweep_spec_start_index=lambda selected: (
+            G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected)
+        ),
+        _prepare_failed_sweep_retry=lambda: prepared.append(True),
+        _begin_return_baseline=lambda after: transitions.append(after),
+        _pause=lambda reason: pauses.append(reason),
+    )
+
+    G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure(
+        node,
+        spec,
+        [{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}],
+    )
+
+    assert prepared == [True]
+    assert transitions == ["resume_sweep"]
+    assert pauses == []
+    assert node.paused_reason == "joint_fit_check_failed"
+
+
+def test_near_threshold_provisional_failure_warns_without_retry(tmp_path) -> None:
+    spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
+    node = SimpleNamespace(
+        raw_path=tmp_path / "raw_samples.jsonl",
+        provisional_warning_ratio=1.25,
+        reason="",
+    )
+    handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure(
+        node,
+        spec,
+        [
+            {
+                "joint": "index_mcp_roll",
+                "metric": "hysteresis_deg",
+                "actual": 5.5,
+                "limit": 5.0,
+                "comparison": "maximum",
+            }
+        ],
+        allow_warning=True,
+    )
+
+    assert handled is False
+    assert node.reason == "provisional_fit_warning_continuing"
+    event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
+    assert event["kind"] == "provisional_fit_warning"
+
+
+def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None:
+    commands: list[list[int]] = []
+    pauses: list[str] = []
+    holds: list[bool] = []
+    node = SimpleNamespace(
+        motion_retry_counts={},
+        automatic_motion_retry_limit=2,
+        raw_path=tmp_path / "raw_samples.jsonl",
+        state="RETURN_BASELINE",
+        baseline_command=tuple([255] * 20),
+        position_hold_since=1.0,
+        reason="",
+        _publish_hold_current=lambda: holds.append(True),
+        _publish_speed_profile=lambda profile: None,
+        _normal_speed_profile=lambda: [15] * 5,
+        _publish_command=lambda command: commands.append(command),
+        _pause=lambda reason: pauses.append(reason),
+        _baseline_error_u8=lambda: 50.0,
+        _reset_motion_progress=lambda now, error: None,
+    )
+
+    for now in (10.0, 20.0, 30.0):
+        G20ThreeCameraCalibrationNode._retry_motion_or_pause(
+            node, "return_baseline_timeout", now
+        )
+
+    assert len(commands) == 2
+    assert holds == []
+    assert pauses == ["return_baseline_timeout"]
+
+
+def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None:
+    pauses: list[str] = []
+    node = SimpleNamespace(
+        raw_path=tmp_path / "raw_samples.jsonl",
+        latest_state_u8=tuple([18.0] + [255.0] * 19),
+        motion_progress_reference_error_u8=18.0,
+        motion_last_progress_at=10.0,
+        motor_stall_timeout_seconds=8.0,
+        motor_stall_minimum_progress_u8=1.0,
+        _pause=lambda reason: pauses.append(reason),
+        _reset_motion_progress=lambda now, error: None,
+    )
+
+    assert not G20ThreeCameraCalibrationNode._pause_if_motion_stalled(
+        node, now=17.9, error_u8=18.0, context="sweep_motor_0"
+    )
+    assert G20ThreeCameraCalibrationNode._pause_if_motion_stalled(
+        node, now=18.0, error_u8=18.0, context="sweep_motor_0"
+    )
+
+    assert pauses == ["motor_state_stalled:sweep_motor_0:error_u8=18.000"]
+    event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
+    assert event["kind"] == "mechanical_motion_stall"
+    assert event["context"] == "sweep_motor_0"
+
+
+def test_slower_sweep_retry_extends_timeout_inversely() -> None:
+    spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
+    item = SweepItem(spec, 0, DIRECTION_DECREASING)
+    key = (spec.motor_index, 0, DIRECTION_DECREASING)
+    node = SimpleNamespace(
+        active_sweep=item,
+        sweep_timeout_seconds=90.0,
+        sweep_retry_counts={key: 1},
+        retry_speed_scales=(0.8, 0.6, 0.5),
+    )
+
+    assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
+        node
+    ) == 112.5
+    node.sweep_retry_counts[key] = 3
+    assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
+        node
+    ) == 180.0
+
+
 def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None:
     spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
     node = _fit_check_node(
@@ -379,6 +937,8 @@ def test_thumb_ip_axis_uses_same_cycle_thumb_mcp_direction() -> None:
     upstream = node._fit_axis_measurement("thumb_mcp", 0)
     passive = node._fit_axis_measurement("thumb_ip", 0)
 
+    assert upstream.condition_command_u8 is not None
+    assert upstream.condition_command_u8[15] == 255.0
     assert abs(
         float(
             np.asarray(upstream.axis_common_xyz)

src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py

@@ -22,6 +22,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
     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 (
@@ -30,6 +31,7 @@ from g20_thumb_apriltag_calibration.full_hand import (
     MEASURED_JOINTS,
     PASSIVE_JOINTS,
     JointCurveFit,
+    get_hand_calibration_profile,
 )
 
 
@@ -38,6 +40,10 @@ 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():
@@ -58,6 +64,18 @@ def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
     ) == 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(),
@@ -139,7 +157,10 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
     assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
 
 
-def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> None:
+@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)
@@ -159,7 +180,7 @@ def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> None
         biased_records.append(biased)
 
     measurement = fit_joint_axis_measurement(
-        "thumb_cmc_pitch", biased_records, cycle=0, zero_command_u8=255
+        joint, biased_records, cycle=0, zero_command_u8=255
     )
 
     observed_axis = np.asarray(measurement.axis_common_xyz)
@@ -175,6 +196,44 @@ def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> None
     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)
@@ -349,6 +408,14 @@ def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Pa
             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(
@@ -383,42 +450,12 @@ def test_urdf_writer_changes_only_the_16_active_zero_origins(
         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)
-
-
-def test_urdf_writer_expands_only_requested_limits_with_margin(
-    tmp_path: Path,
-) -> None:
-    margin = math.radians(0.5)
-    measured_ranges = {
-        "index_mcp_roll": (-0.25113634, 0.24813698),
-        "index_mcp_pitch": (0.0, 1.22424601),
-        "index_pip": (0.0, 1.74215779),
-    }
-    destination = write_zero_corrected_urdf(
-        source_urdf=SOURCE_URDF,
-        output_directory=tmp_path,
-        serial_number="G20_LEFT_001",
-        offsets_rad={"thumb_cmc_yaw": math.radians(1.0)},
-        timestamp="20260807_181500",
-        joint_angle_ranges_rad=measured_ranges,
-        joint_limit_margin_rad=margin,
-    )
-
-    roll_lower, roll_upper = _joint_limit(destination, "index_mcp_roll")
-    pitch_lower, pitch_upper = _joint_limit(destination, "index_mcp_pitch")
-    pip_lower, pip_upper = _joint_limit(destination, "index_pip")
-    assert roll_lower == pytest.approx(measured_ranges["index_mcp_roll"][0] - margin)
-    assert roll_upper == pytest.approx(measured_ranges["index_mcp_roll"][1] + margin)
-    assert pitch_lower == pytest.approx(_joint_limit(SOURCE_URDF, "index_mcp_pitch")[0])
-    assert pitch_upper == pytest.approx(measured_ranges["index_mcp_pitch"][1] + margin)
-    assert pip_lower == pytest.approx(_joint_limit(SOURCE_URDF, "index_pip")[0])
-    assert pip_upper == pytest.approx(measured_ranges["index_pip"][1] + margin)
-    assert _joint_limit(destination, "thumb_cmc_yaw") == pytest.approx(
-        _joint_limit(SOURCE_URDF, "thumb_cmc_yaw")
-    )
-    assert _joint_limit(destination, "middle_mcp_roll") == pytest.approx(
-        _joint_limit(SOURCE_URDF, "middle_mcp_roll")
-    )
+    # 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:
@@ -438,7 +475,306 @@ def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
     )
 
 
+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 = {
@@ -454,7 +790,7 @@ def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
     true_offsets = {
         name: math.radians(value)
         for name, value in zip(
-            DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 0.0, 0.0, 0.0, 0.0]
+            DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
         )
     }
     model = UrdfKinematicModel(SOURCE_URDF)
@@ -514,37 +850,128 @@ def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
         )
     ) > 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=1.0e-12
+        0.0, abs=math.radians(0.05)
     )
-    for target, source in INHERITED_ZERO_JOINTS.items():
+    for target in INHERITED_ZERO_JOINTS:
         assert result.all_active_offsets_rad[target] == pytest.approx(
-            result.direct_offsets_rad[source], abs=1.0e-12
-        )
-    for joint in (
-        "index_mcp_roll",
-        "middle_mcp_roll",
-        "ring_mcp_roll",
-        "pinky_mcp_roll",
-    ):
-        assert result.all_active_offsets_rad[joint] == pytest.approx(
             0.0, abs=1.0e-12
         )
-    for joint in (
-        "index_mcp_pitch",
-        "middle_mcp_pitch",
-        "ring_mcp_pitch",
-        "pinky_mcp_pitch",
-    ):
-        assert result.all_active_offsets_rad[joint] == pytest.approx(
-            0.0, abs=1.0e-12
-        )
-    for joint in ("index_pip", "middle_pip", "ring_pip", "pinky_pip"):
-        assert result.all_active_offsets_rad[joint] == pytest.approx(
+    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/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],