diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 3794598..55aade3 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -15,18 +15,34 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right 当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量 独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面 -校验视角的任务级有效率只记录为诊断，完整端点、分箱覆盖和模型质量门限仍保持 -不变。需要强制从第一个关节 +轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次 +采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面 +校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值， +但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整 +轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量 +门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一 +采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用 +产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态 +过程。需要强制从第一个关节 重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为 一个完整同步任务断点；只有两边数据都完整时才复用。 命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 -15个会话数据求解主动关节URDF零位修正（`thumb_mcp`固定CAD参考）、 +16个会话数据求解主动关节URDF零位修正、 21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。 终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。 四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag 平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。 +侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用 +静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。 +单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只 +限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧 +绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。 +终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。 +若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存 +`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参 +哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。 正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在 JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证 @@ -64,19 +80,22 @@ ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \ 固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中 允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不 放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的 -实时Tag。锁定后不得移动相机、手掌底座或整只手，否则缓存参考失效，必须重新启动 -标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计； +实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考 +失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点 +零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计； 任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把 采集质量良好的任务误判为可见性失败。 恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次 相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端 -能够触掌是独立的机械端点约束。因此四指MCP pitch、PIP的URDF静态零位由本次 -实测全屈曲行程与CAD触掌角之差求出，不再把同相机的平行轴PnP相位直接当作绝对 -零位，也不写死为0。生成URDF时同步修正这些关节的坐标上限和DIP mimic坐标偏置， +能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch +和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的 +平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的 +坐标上限以及相关被动关节的mimic坐标偏置， 保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和 -第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、四指MCP roll、 -MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。 +第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、 +四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用 +实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。 侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出； 同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时 运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” @@ -87,7 +106,8 @@ MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。 当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机 退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的 "滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺 -不超过8，会把非roll任务四轮正式速度最多提高到预检速度的1.5倍；否则保持原保守速度。 +不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值， +不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。 四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理 零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、 四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。 @@ -96,7 +116,8 @@ MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。 方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再 替代静态保持姿态。 前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。 -每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化，但同一任务第1轮 +每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式 +扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮 已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的 IPPE镜像解。baseline标准接近和全部质量门限保持不变。 电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中 @@ -105,26 +126,31 @@ IPPE镜像解。baseline标准接近和全部质量门限保持不变。 当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。 URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个， -其中四指`mcp_pitch/pip`共8个字段由视觉行程+机械端点联合求解，只有`thumb_mcp` -字段固定为0； +其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程 +与机械端点联合求解； +写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的 +`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态； `thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容， 当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、 轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中 相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入 数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测 -前后轴向稳定相差11.4°），因此跨视角方向差超过0.75°融合门限时不再判定任务 -失败：程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的 -前视轴向；仅当差值超过粗错误兜底上限（`cross_view_roll_maximum_axis_difference_deg` -默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm， -对应标签贴错连杆或标签松动）时才失败。前视侧摆连杆与侧视PIP连杆的轴线 -本身存在约21mm的系统性位置差（丝杠平移连杆），属预期现象。侧视校验通道 +前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收， +正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中 +严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置， +避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是 +随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限； +两者线距仅记录在诊断中。侧视校验通道 （`*_mcp_roll_side`）的 -分支间隙跨轮极差上限放宽为 -`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°（绝对间隙1.5°上限 -不变），其`axis_pose_line_rms`降为诊断，不再作为准入门限；径向、平面、 -圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受 +姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自 +近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴 +仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮 +位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。 +侧面端视roll的圆轨迹方向已经受 姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限； -径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、 +径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的 +二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮 +留出失败仍会拒绝发布。任一静态目标、第四轮留出、 遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。 8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔 位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 @@ -335,13 +361,14 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn 约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。 四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断， -但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零 -修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何 -按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。 +但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测 +装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械 +端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或 +序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。 -7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束 -拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴 -约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。 +视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指 +pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点 +的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。 原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5 保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。 偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL @@ -431,9 +458,9 @@ calibration_output/G20_RIGHT_001/<时间戳>/ `zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从 指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或 覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能 -改变；当前19-Tag右手保留16个主动静态零位字段，其中15个由本会话数据求解，只有 -`thumb_mcp`固定为原始CAD零位0；四指DIP为被动关节，发布实测动态曲线并保留CAD -静态零位，其mimic坐标偏置只随上游PIP坐标系变换作等价调整。 +改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和 +`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随 +上游主动关节坐标系变换作等价调整。 未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到 同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。 diff --git a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml index c8753fe..07f3f5a 100644 --- a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml +++ b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml @@ -1,6 +1,7 @@ schema_version: 1 model: G20 side: right +tag_layout: g20_right_19 serial_number: G20_RIGHT_001 can_interface: can0 output_root: calibration_output @@ -23,9 +24,9 @@ artifacts: source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8 camera_extrinsics: config/g20_three_camera_extrinsics.yaml - camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a + camera_extrinsics_sha256: 59ab7510ad0a2912ca636876039427c4c25906cbbfa84e96300d46b86e929334 calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml - calibration_config_sha256: d1a8be37c9ae21cb4f975ddd66c3cf93fed25dae98ddcf971259b395fa69b521 + calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml index 961a216..0aa2473 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -84,10 +84,12 @@ g20_calibration: zero_maximum_axis_cone_mismatch_deg: 5.0 zero_maximum_observability_condition_number: 10000000000.0 zero_maximum_offset_deg: 20.0 - # 四指MCP roll保留严格的装配保护范围。MCP pitch/PIP静态零位由 - # 实测全屈曲行程与反馈0触掌的CAD机械端点联合求解，不允许写死为0。 + # 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何 + # 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP + # 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。 zero_finger_maximum_offset_deg: 3.0 - flexion_endpoint_maximum_offset_deg: 5.0 + # 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。 + mechanical_endpoint_maximum_offset_deg: 5.0 endpoint_tolerance_u8: 2.0 # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈 @@ -155,6 +157,11 @@ g20_calibration: directional_zero_maximum_branch_gap_deg: 2.0 directional_zero_maximum_branch_gap_range_deg: 0.3 cross_view_roll_maximum_branch_gap_difference_deg: 0.3 + # 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的 + # 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状； + # 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。 + cross_view_roll_maximum_shape_rms_deg: 1.25 + cross_view_roll_maximum_projection_scale_ratio: 1.5 passive_maximum_monotonic_correction_deg: 3.0 passive_maximum_hysteresis_deg: 2.0 # 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py index 1524dfd..952da99 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py @@ -17,6 +17,7 @@ import xml.etree.ElementTree as ET import numpy as np +from .core import COMMAND_NAMES from .trajectory import ( _angle_for_circle, _fit_circle_with_axis, @@ -72,6 +73,24 @@ class SweepSpec: return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" +@dataclass(frozen=True) +class PalmAxisObserver: + """Non-blocking direction observation attached to an existing task. + + This is deliberately not a ``JointSpec``: it has no curve, endpoint or + retry semantics. The camera callback records it only while both Tags are + visible during the named sweep. + """ + + source_name: str + task_name: str + view: str + parent_role: str + child_role: str + model_joint: str + motor_index: int + + @dataclass(frozen=True) class JointCurveFit: angle_rad: tuple[float, ...] @@ -98,6 +117,23 @@ class HandCalibrationProfile: layout_id: str = "legacy_11" measurement_specs: Mapping[str, JointSpec] | None = None axis_validation_sources: Mapping[str, str] | None = None + palm_axis_observers: tuple[PalmAxisObserver, ...] = () + minimum_palm_orientation_sources: int = 0 + # Hardware and algorithm differences belong to the product profile, not + # to the acquisition state machine. Keeping these fields here lets a new + # model (or the mirrored hand) reuse the same recorder/fitter by supplying + # a different declarative profile. + model: str = "G20" + command_names: tuple[str, ...] = COMMAND_NAMES + baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND + capabilities: frozenset[str] = frozenset() + + @property + def command_count(self) -> int: + return len(self.command_names) + + def supports(self, capability: str) -> bool: + return str(capability) in self.capabilities @property def measured_joints(self) -> tuple[str, ...]: @@ -115,6 +151,20 @@ class HandCalibrationProfile: name for name, spec in self.record_specs.items() if spec.measured ) + @property + def palm_orientation_sources(self) -> Mapping[str, str]: + return { + observer.source_name: observer.model_joint + for observer in self.palm_axis_observers + } + + @property + def palm_axis_motor_by_source(self) -> Mapping[str, int]: + return { + observer.source_name: int(observer.motor_index) + for observer in self.palm_axis_observers + } + @property def active_joints(self) -> tuple[str, ...]: return tuple( @@ -612,6 +662,20 @@ def _build_right_19_profile() -> HandCalibrationProfile: layout_id=G20_RIGHT_19_LAYOUT, measurement_specs=measurement_specs, axis_validation_sources=axis_validation_sources, + palm_axis_observers=(), + minimum_palm_orientation_sources=0, + capabilities=frozenset( + { + "precheck_sweeps", + "steady_command_checkpoints", + "directional_zero", + "isolated_holdout", + "cross_view_roll_curve", + "stable_cross_view_cone_bias", + "measured_passive_dips", + "urdf_zero_publication", + } + ), ) @@ -1435,18 +1499,86 @@ def compare_cross_view_roll_curves( *, maximum_rms_difference_rad: float = math.radians(1.0), maximum_branch_gap_difference_rad: float = math.radians(0.3), + allow_projection_scale: bool = False, + maximum_projection_scale_ratio: float = 1.5, ) -> dict[str, float]: - """Validate two independently fitted roll curves before axis fusion.""" + """Validate two independently fitted roll curves before axis fusion. + + The G20 right roll primary is an image-plane centre trajectory while its + side validation is a 3-D rotation trajectory. A fixed Tag mounting tilt + changes the image-plane angular scale without changing the mechanism. In + projection-scale mode, remove that one constant scale before comparing + curve shape. Direction, scale bounds, direction-dependent branch gap and + every single-view quality/holdout gate remain independent hard checks. + """ if maximum_rms_difference_rad <= 0.0: raise ValueError("cross-view RMS limit must be positive") if maximum_branch_gap_difference_rad <= 0.0: raise ValueError("cross-view branch-gap limit must be positive") - metrics: dict[str, float] = {} + if maximum_projection_scale_ratio <= 1.0: + raise ValueError("cross-view projection-scale ratio must exceed one") + + fields: dict[str, tuple[np.ndarray, np.ndarray]] = {} for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): left = np.asarray(getattr(primary, field_name), dtype=float) right = np.asarray(getattr(secondary, field_name), dtype=float) if left.shape != (256,) or right.shape != (256,): raise ValueError("cross-view curves must contain 256 values") + if not np.all(np.isfinite(left)) or not np.all(np.isfinite(right)): + raise ValueError("cross-view curves must be finite") + fields[field_name] = (left, right) + + primary_travel = float( + fields["angle_rad"][0][0] - fields["angle_rad"][0][255] + ) + secondary_travel = float( + fields["angle_rad"][1][0] - fields["angle_rad"][1][255] + ) + if primary_travel * secondary_travel <= 0.0: + raise ValueError("cross_view_roll_curve_direction_disagrees") + + primary_zero = int(primary.circle.get("zero_command_u8", 127)) + secondary_zero = int(secondary.circle.get("zero_command_u8", 127)) + if not 0 <= primary_zero <= 255 or not 0 <= secondary_zero <= 255: + raise ValueError("cross-view zero command must be in [0, 255]") + primary_anchor = float(primary.decreasing_rad[primary_zero]) + secondary_anchor = float(secondary.decreasing_rad[secondary_zero]) + projection_scale = 1.0 + if allow_projection_scale: + primary_values = np.concatenate( + [left - primary_anchor for left, _ in fields.values()] + ) + secondary_values = np.concatenate( + [right - secondary_anchor for _, right in fields.values()] + ) + denominator = float(secondary_values @ secondary_values) + if denominator <= np.finfo(float).eps: + raise ValueError("cross_view_roll_projection_scale_unobservable") + projection_scale = float( + (primary_values @ secondary_values) / denominator + ) + if projection_scale <= 0.0 or not math.isfinite(projection_scale): + raise ValueError("cross_view_roll_curve_direction_disagrees") + scale_ratio = max(projection_scale, 1.0 / projection_scale) + if scale_ratio > float(maximum_projection_scale_ratio): + raise ValueError( + "cross_view_roll_projection_scale_ratio_too_large:" + f"{scale_ratio:.6f}" + ) + + metrics: dict[str, float] = { + "projection_scale": projection_scale, + "projection_scale_ratio": max( + projection_scale, 1.0 / projection_scale + ), + } + for field_name, (left, right) in fields.items(): + metrics[f"raw_{field_name}_rms_difference_rad"] = float( + np.sqrt(np.mean(np.square(left - right))) + ) + if allow_projection_scale: + left = left - primary_anchor + right = projection_scale * (right - secondary_anchor) rms = float(np.sqrt(np.mean(np.square(left - right)))) metrics[f"{field_name}_rms_difference_rad"] = rms if rms > maximum_rms_difference_rad: @@ -1454,13 +1586,7 @@ def compare_cross_view_roll_curves( "cross_view_roll_curve_difference_too_large:" f"{field_name}:{math.degrees(rms):.6f}deg" ) - primary_travel = float(primary.angle_rad[0] - primary.angle_rad[255]) - secondary_travel = float(secondary.angle_rad[0] - secondary.angle_rad[255]) - if primary_travel * secondary_travel <= 0.0: - raise ValueError("cross_view_roll_curve_direction_disagrees") metrics["travel_difference_rad"] = abs(primary_travel - secondary_travel) - primary_zero = int(primary.circle.get("zero_command_u8", 127)) - secondary_zero = int(secondary.circle.get("zero_command_u8", 127)) primary_gap = float( primary.increasing_rad[primary_zero] - primary.decreasing_rad[primary_zero] @@ -1469,9 +1595,27 @@ def compare_cross_view_roll_curves( secondary.increasing_rad[secondary_zero] - secondary.decreasing_rad[secondary_zero] ) - branch_gap_difference = abs(primary_gap - secondary_gap) + metrics["raw_baseline_branch_gap_difference_rad"] = abs( + primary_gap - secondary_gap + ) + branch_gap_difference = abs( + primary_gap - projection_scale * secondary_gap + ) metrics["baseline_branch_gap_difference_rad"] = branch_gap_difference - if branch_gap_difference > maximum_branch_gap_difference_rad: + # In projection-scale mode the primary is an image-circle phase while the + # secondary is a 3-D rotation angle. A fixed Tag tilt/offset changes the + # image projection of the two motion branches independently, so their + # absolute gap is not a cross-view invariant. Both views retain their + # own hard absolute-gap and four-cycle gap-range checks in the acquisition + # quality gate. Keep this value as a diagnostic, but apply the cross-view + # absolute-gap gate only when both curves live in comparable coordinates. + metrics["baseline_branch_gap_gate_applied"] = float( + not allow_projection_scale + ) + if ( + not allow_projection_scale + and branch_gap_difference > maximum_branch_gap_difference_rad + ): raise ValueError( "cross_view_roll_branch_gap_difference_too_large:" f"{math.degrees(branch_gap_difference):.6f}deg" @@ -1479,6 +1623,35 @@ def compare_cross_view_roll_curves( return metrics +def cross_view_roll_diagnostic_metrics( + primary: JointCurveFit, + secondary: JointCurveFit, +) -> dict[str, float]: + """Compare a validation-only roll view without creating a retry gate. + + G20-right publishes the front image-plane roll curve. The side PIP-link + observation supplies an independently quality-gated physical axis-line + position, but its angle includes projection and linkage effects and is + therefore diagnostic rather than a second measurement of the published + curve. Live fitting and offline replay must use the same non-blocking + comparison; otherwise a stable view bias can recall an already complete + task even though reacquiring the same motion cannot change the result. + """ + try: + return compare_cross_view_roll_curves( + primary, + secondary, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=True, + maximum_projection_scale_ratio=float("inf"), + ) + except ValueError as error: + if str(error) == "cross_view_roll_curve_direction_disagrees": + return {"direction_disagrees": 1.0} + raise + + def fit_projected_zero( records: Sequence[Mapping[str, Any]], *, diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py index 7e01f7d..9b8eca2 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py @@ -30,25 +30,34 @@ from .full_hand import ( build_compact_payload, canonical_zero_direction, clamp_runtime_fits_to_urdf_limits, - compare_cross_view_roll_curves, + cross_view_roll_diagnostic_metrics, derive_mimic_passive_fits, fit_joint_image_curve, get_hand_calibration_profile, validate_compact_payload, ) +from .product import get_product_calibration_contract from .storage import atomic_write_json from .urdf_zero import ( JointAxisMeasurement, UrdfKinematicModel, _angles_from_state, + anchor_right_19_mechanical_endpoint_curves, + axis_line_uses_depth_free_interpretation_plane, + axis_line_cycle_rms_m, baseline_hysteresis_by_cycle_rad, circle_direction_is_constrained, + cross_view_side_line_source, fit_joint_axis_measurement, + fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, get_zero_calibration_profile, - derive_right_19_flexion_endpoint_offsets, + derive_right_19_mechanical_endpoint_offsets, joint_curve_holdout_errors, + refit_axis_line_group_with_shared_radius, + select_cross_view_roll_direction_source, solve_urdf_zero_offsets, + with_depth_free_axis_projection, write_zero_corrected_urdf, ) @@ -139,6 +148,35 @@ def _latest_attempt_records( return dict(records) +def _latest_palm_axis_records( + rows: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + """Select the latest accepted append-only palm side-channel attempts.""" + samples = [ + dict(row) for row in rows if row.get("kind") == "palm_axis_sample" + ] + latest_attempt: dict[tuple[str, int, str], int] = {} + for row in samples: + key = ( + str(row["source_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["source_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[ @@ -146,6 +184,7 @@ def _load_raw_session( dict[str, list[dict[str, Any]]], dict[str, list[dict[str, Any]]], dict[str, list[dict[str, Any]]], + dict[str, list[dict[str, Any]]], Path, ]: raw_path = session_dir / "raw_samples.jsonl" @@ -163,7 +202,9 @@ def _load_raw_session( for row in rows: if "command_u8" in row: continue - if row.get("kind") in {"sample", "baseline_hold_sample"}: + if row.get("kind") in { + "sample", "baseline_hold_sample", "palm_axis_sample" + }: row["command_u8"] = int(round(float(row["feedback_u8"]))) elif row.get("kind") == "steady_command_sample": row["command_u8"] = int(row["requested_command_u8"]) @@ -175,6 +216,7 @@ def _load_raw_session( _latest_attempt_records(rows), _latest_attempt_records(rows, kind="baseline_hold_sample"), _latest_attempt_records(rows, kind="steady_command_sample"), + _latest_palm_axis_records(rows), raw_path, ) @@ -189,7 +231,13 @@ def _fit_curve( motor = profile.record_specs[name].motor_index if ( profile.layout_id == G20_RIGHT_19_LAYOUT - and name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + and ( + name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + or ( + name.endswith("_mcp_roll") + and not name.startswith("thumb_") + ) + ) ): return fit_joint_image_curve(records) return fit_rotation_joint_curve( @@ -234,6 +282,7 @@ def _fit_axes( ) extrinsics = load_three_camera_extrinsics(extrinsics_file) cache: dict[tuple[str, int], JointAxisMeasurement] = {} + cross_view_direction_source_cache: dict[str, str] = {} upstream_by_joint = { "thumb_mcp": "thumb_cmc_pitch", "thumb_ip": "thumb_mcp", @@ -246,6 +295,15 @@ def _fit_axes( for finger in ("index", "middle", "ring", "pinky") }, } + if profile.layout_id == G20_RIGHT_19_LAYOUT: + reference_roll = f"{profile.reference_finger}_mcp_roll" + upstream_by_joint.update( + { + f"{finger}_mcp_roll": reference_roll + for finger in ("index", "middle", "ring", "pinky") + if f"{finger}_mcp_roll" != reference_roll + } + ) def fit_raw(name: str, cycle: int) -> JointAxisMeasurement: key = (name, cycle) @@ -288,6 +346,9 @@ def _fit_axes( ), view_normal_common_xyz=tuple(float(value) for value in view_normal), ) + result = with_depth_free_axis_projection( + result, extrinsics.transform(spec.view)[:3, 3] + ) cache[key] = result return result @@ -311,11 +372,52 @@ def _fit_axes( np.cross(secondary_point - primary_point, primary_axis) ) ) + if ( + profile.record_specs[validation_name].zero_kind + == "axis_cross_view_validation" + ): + point_view = profile.record_specs[name].view + if point_view is None: + raise ValueError( + f"{name}: primary point source has no view" + ) + point_camera_center = extrinsics.transform(point_view)[:3, 3] + point_ray = primary_point - point_camera_center + interpretation_plane_normal = np.cross( + point_ray, primary_axis + ) + interpretation_plane_normal /= np.linalg.norm( + interpretation_plane_normal + ) + return replace( + primary, + point_common_xyz_m=tuple( + float(value) + for value in primary.point_common_xyz_m + ), + pose_axis_line_rms_m=primary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(name,), + axis_point_source=( + "front_interpretation_plane_cross_view_validated" + ), + axis_point_camera_center_common_xyz_m=tuple( + float(value) + for value in point_camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) + for value in interpretation_plane_normal + ), + ) # Mirror the live node's tiered policy: the side-view planar-tag # IPPE bias makes sub-degree cross-view agreement unreachable, so # disagreement above the fusion gate keeps the front-only axis while - # only the gross bound (wrong link / loose tag) still fails. - if difference > math.radians(15.0) or line_distance > 0.030: + # only a gross direction error (wrong link / loose tag) still fails. + # The front screw-driven roll trajectory yields a displaced + # pseudo-line, so its distance to the side physical line is retained + # only as a diagnostic. The live node likewise keeps only the gross + # direction gate here and validates side-line quality independently. + if difference > math.radians(15.0): raise ValueError(f"{name}: cross-view axis gross disagreement") if difference > math.radians(0.75) or line_distance > 0.001: selected_direction = primary_axis @@ -329,7 +431,6 @@ def _fit_axes( None, ) if observer_name is not None: - observer = fit_raw(observer_name, cycle) model = UrdfKinematicModel(source_urdf) parent_axis, _ = model.axis_line( name, zero_offsets={}, joint_angles={} @@ -340,11 +441,9 @@ def _fit_axes( expected_cone = math.acos( abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0))) ) - measured_observer = np.asarray( - observer.axis_common_xyz, dtype=float - ) - - def cone_residual(candidate: np.ndarray) -> float: + def cone_residual( + candidate: np.ndarray, measured_observer: np.ndarray + ) -> float: measured_cone = math.acos( abs( float( @@ -356,10 +455,46 @@ def _fit_axes( ) return abs(measured_cone - expected_cone) - if ( - cone_residual(primary_axis) > math.radians(5.0) - and cone_residual(secondary_axis) <= math.radians(5.0) - ): + if name not in cross_view_direction_source_cache: + primary_residuals: list[float] = [] + secondary_residuals: list[float] = [] + for group_cycle in range(repetitions): + group_observer = np.asarray( + fit_raw( + observer_name, group_cycle + ).axis_common_xyz, + dtype=float, + ) + primary_residuals.append( + cone_residual( + np.asarray( + fit_raw( + name, group_cycle + ).axis_common_xyz, + dtype=float, + ), + group_observer, + ) + ) + secondary_residuals.append( + cone_residual( + np.asarray( + fit_raw( + validation_name, group_cycle + ).axis_common_xyz, + dtype=float, + ), + group_observer, + ) + ) + cross_view_direction_source_cache[name] = ( + select_cross_view_roll_direction_source( + primary_residuals, + secondary_residuals, + math.radians(5.0), + ) + ) + if cross_view_direction_source_cache[name] == "secondary": selected_direction = secondary_axis selected_source = "cross_view_cone_selected_secondary" # Mirror the live node: use the view whose direction satisfies @@ -375,6 +510,7 @@ def _fit_axes( for value in secondary.point_common_xyz_m ), pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(validation_name,), axis_direction_source=selected_source, axis_point_source="side_circle_cross_view", ) @@ -406,14 +542,45 @@ def _fit_axes( secondary.pose_axis_line_rms_m, line_distance, ), + pose_axis_line_source_joints=(name, validation_name), axis_direction_source="cross_view_weighted_fusion", ) - return [ - fit_one(name, cycle) - for name in zero_profile.axis_joints - for cycle in range(repetitions) - ] + result: list[JointAxisMeasurement] = [] + for name in zero_profile.axis_joints: + group = [fit_one(name, cycle) for cycle in range(repetitions)] + side_sources = { + source + for measurement in group + if (source := cross_view_side_line_source(measurement)) is not None + } + if ( + len(side_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in group + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in group + ) + ): + source = next(iter(side_sources)) + source_spec = profile.record_specs[source] + group = list( + refit_axis_line_group_with_shared_radius( + group, + records_by_joint[source], + zero_command_u8=int( + baseline[source_spec.motor_index] + ), + canonical_zero_direction=canonical_zero_direction( + profile, source + ), + ) + ) + result.extend(group) + return result def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float: @@ -601,6 +768,7 @@ def _quality_failures( if ( max(baseline_hysteresis) - min(baseline_hysteresis) > gap_range_limit + and spec.zero_kind != "axis_cross_view_validation" ): failures.append( f"{name}: baseline directional gap repeatability" @@ -613,6 +781,7 @@ def _quality_failures( cycle_travels: list[float] = [] cycle_axes: list[Sequence[float]] = [] cycle_axis_sources: list[str] = [] + cycle_axis_measurements: list[JointAxisMeasurement] = [] for cycle in range(repetitions): cycle_fit = _fit_curve( name, @@ -626,11 +795,13 @@ def _quality_failures( axis = axis_by_key[(name, cycle)] cycle_axes.append(axis.axis_common_xyz) cycle_axis_sources.append(axis.axis_direction_source) + cycle_axis_measurements.append(axis) if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]): failures.append(f"{name} cycle {cycle + 1}: radial RMS") if ( profile.record_specs[name].zero_kind != "axis_cross_view_validation" + and cross_view_side_line_source(axis) is None and axis.pose_axis_line_rms_m > float(parameters["axis_maximum_pose_line_rms_m"]) ): @@ -655,6 +826,29 @@ def _quality_failures( ) ): failures.append(f"{name} cycle {cycle + 1}: axis disagreement") + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if ( + len(cycle_axis_measurements) == repetitions + and len(side_line_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in cycle_axis_measurements + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in cycle_axis_measurements + ) + and axis_line_cycle_rms_m(cycle_axis_measurements) + > float(parameters["axis_maximum_pose_line_rms_m"]) + ): + source = next(iter(side_line_sources)) + failures.append(f"{source}: axis-line cycle RMS") travel_limit = math.radians( float( parameters[ @@ -671,6 +865,7 @@ def _quality_failures( source == "upstream_constraint" for source in cycle_axis_sources ) + and spec.zero_kind != "axis_cross_view_validation" and _maximum_undirected_axis_difference(cycle_axes) > math.radians( float(parameters["zero_maximum_axis_cycle_difference_deg"]) ) @@ -832,21 +1027,28 @@ def replay_session( records_by_joint, baseline_records_by_joint, command_records_by_joint, + palm_axis_records_by_source, raw_path, ) = _load_raw_session(session) + model = str(start.get("model", "G20")).strip().upper() side = str(start["hand_type"]).lower() requested_layout_id = str( start.get("tag_layout", "legacy_11") ).lower() - profile = get_hand_calibration_profile(side, requested_layout_id) + contract = get_product_calibration_contract( + model, side, requested_layout_id + ) + profile = contract.profile # Normalise the former g20_right_15 compatibility alias immediately so # every downstream product guard uses the actual physical 19-Tag profile # instead of silently falling through legacy-11 logic. layout_id = profile.layout_id - zero_profile = get_zero_calibration_profile(side, layout_id) + zero_profile = contract.zero_profile 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 len(baseline) != profile.command_count: + raise ValueError( + "session baseline length differs from the registered product schema" + ) if ( layout_id == G20_RIGHT_19_LAYOUT and write_outputs @@ -878,8 +1080,14 @@ def replay_session( for row in combinations ): raise ValueError("combination pose validation exceeds product limits") - if set(records_by_joint) != set(profile.record_joints): - raise ValueError("raw session does not contain exactly the task record set") + required_record_joints = set(profile.record_joints) + if ( + not required_record_joints.issubset(records_by_joint) + or not set(records_by_joint).issubset(set(profile.record_joints)) + ): + raise ValueError( + "raw session does not contain the required task record set" + ) def hysteresis_records( name: str, @@ -904,7 +1112,9 @@ def replay_session( name: str, *, require_full_pose: bool = False ) -> list[dict[str, Any]]: """Merge settled baseline holds into directional model records.""" - records = [dict(record) for record in records_by_joint[name]] + records = [ + dict(record) for record in records_by_joint.get(name, ()) + ] if canonical_zero_direction(profile, name) is None: return records required = { @@ -971,7 +1181,7 @@ def replay_session( } command_fits = dict(training_fits) if layout_id == G20_RIGHT_19_LAYOUT: - if set(command_records_by_joint) != set(profile.record_joints): + if set(command_records_by_joint) != required_record_joints: raise ValueError( "g20_right_19 session is incomplete: missing steady command checkpoints" ) @@ -1035,6 +1245,11 @@ def replay_session( command_fits, profile=profile, ) + command_fits = anchor_right_19_mechanical_endpoint_curves( + command_fits, + command_records_by_joint, + maximum_direction_difference_rad=command_gap_limit, + ) cross_view_roll_metrics: dict[str, dict[str, float]] = {} for name, validation_name in ( profile.axis_validation_sources or {} @@ -1055,35 +1270,11 @@ def replay_session( profile=profile, baseline=baseline, ) - training_metrics = compare_cross_view_roll_curves( - training_fits[name], - validation_training, - maximum_rms_difference_rad=math.radians( - float(parameters["maximum_validation_mae_deg"]) - ), - maximum_branch_gap_difference_rad=math.radians( - float( - parameters.get( - "cross_view_roll_maximum_branch_gap_difference_deg", - 0.3, - ) - ) - ), + training_metrics = cross_view_roll_diagnostic_metrics( + training_fits[name], validation_training ) - final_metrics = compare_cross_view_roll_curves( - measured_fits[name], - validation_all, - maximum_rms_difference_rad=math.radians( - float(parameters["maximum_validation_mae_deg"]) - ), - maximum_branch_gap_difference_rad=math.radians( - float( - parameters.get( - "cross_view_roll_maximum_branch_gap_difference_deg", - 0.3, - ) - ) - ), + final_metrics = cross_view_roll_diagnostic_metrics( + measured_fits[name], validation_all ) cross_view_roll_metrics[name] = { **{ @@ -1103,6 +1294,28 @@ def replay_session( source_urdf=source_urdf, repetitions=repetitions, ) + palm_orientation_measurements, palm_orientation_rejections = ( + fit_partial_palm_orientation_measurements( + sources=profile.palm_orientation_sources, + records_by_joint=palm_axis_records_by_source, + motor_by_source=profile.palm_axis_motor_by_source, + baseline_command_u8=baseline, + cycles=range(repetitions), + minimum_sources=int( + profile.minimum_palm_orientation_sources + ), + minimum_arc_rad=math.radians( + float(parameters["trajectory_minimum_arc_deg"]) + ), + maximum_rotation_orthogonal_rms_rad=math.radians( + float( + parameters[ + "active_maximum_rotation_orthogonal_rms_deg" + ] + ) + ), + ) + ) failures = _quality_failures( records_by_joint=records_by_joint, baseline_records_by_joint=( @@ -1223,11 +1436,15 @@ def replay_session( name: int(spec.motor_index) for name, spec in profile.joint_specs.items() } endpoint_zero_offsets = ( - derive_right_19_flexion_endpoint_offsets( + derive_right_19_mechanical_endpoint_offsets( source_urdf, command_fits, maximum_offset_rad=math.radians( - float(parameters.get("flexion_endpoint_maximum_offset_deg", 5.0)) + float( + parameters.get( + "mechanical_endpoint_maximum_offset_deg", 5.0 + ) + ) ), ) if layout_id == G20_RIGHT_19_LAYOUT @@ -1242,6 +1459,7 @@ def replay_session( solve_arguments = { "source_urdf": source_urdf, "measurements": axes, + "palm_orientation_measurements": palm_orientation_measurements, "motor_by_joint": motor_by_joint, "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])), "finger_maximum_offset_rad": math.radians( @@ -1254,6 +1472,13 @@ def replay_session( "maximum_axis_cone_mismatch_rad": math.radians( float(parameters["zero_maximum_axis_cone_mismatch_deg"]) ), + "maximum_systematic_axis_cone_bias_rad": math.radians( + float( + parameters.get( + "cross_view_roll_maximum_axis_difference_deg", 15.0 + ) + ) + ), "maximum_observability_condition_number": float( parameters.get( "zero_maximum_observability_condition_number", 1.0e10 @@ -1469,6 +1694,21 @@ def replay_session( "offset_covariance_rad2": ( final_zero.offset_covariance_rad2 ), + "palm_orientation_sources": [ + { + "source_joint": item.source_joint, + "model_joint": item.model_joint, + "cycle": item.cycle, + "observed_arc_rad": item.observed_arc_rad, + "rotation_orthogonal_rms_rad": ( + item.rotation_orthogonal_rms_rad + ), + } + for item in palm_orientation_measurements + ], + "palm_orientation_rejections": dict( + palm_orientation_rejections + ), } if layout_id == G20_RIGHT_19_LAYOUT else None @@ -1508,7 +1748,7 @@ def replay_session( "thumb_default": float(parameters["zero_maximum_offset_deg"]), }, "static_zero_policy": ( - "visual_axes_plus_measured_contact_endpoint" + "visual_axes_plus_measured_mechanical_endpoint" if layout_id == G20_RIGHT_19_LAYOUT else "direct_measurements_only" ), @@ -1538,6 +1778,15 @@ def replay_session( name: [math.degrees(value) for value in values] for name, values in holdout_zero.cycle_offsets_rad.items() }, + "axis_cone_mismatch_by_joint_deg": { + name: math.degrees(value) + for name, value in ( + holdout_zero.axis_cone_mismatch_by_joint_rad.items() + ) + }, + "axis_cone_bias_classification_by_joint": dict( + holdout_zero.axis_cone_bias_classification_by_joint + ), "offset_uncertainty_deg": { name: math.degrees(value) for name, value in holdout_zero.offset_uncertainty_rad.items() @@ -1577,7 +1826,7 @@ def replay_session( def main() -> None: parser = argparse.ArgumentParser( - description="Replay and independently validate a complete G20 calibration session." + description="Replay and validate a registered hand-calibration session." ) parser.add_argument("session_dir") parser.add_argument("--serial-number", default=None) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py index 2925ffb..a9fcc4d 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py @@ -1,4 +1,4 @@ -"""One-command product runner for G20_RIGHT_001 calibration.""" +"""One-command runner for a registered hand-calibration product.""" from __future__ import annotations @@ -155,8 +155,9 @@ def _launch_command( resume_from: Path | None = None, ) -> list[str]: values = { - "hand_type": "right", - "tag_layout": "g20_right_19", + "model": config.model, + "hand_type": config.side, + "tag_layout": config.tag_layout, "serial_number": config.serial_number, "can_interface": config.can_interface, "session_dir": str(session), @@ -462,8 +463,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None: continue if ( start is None - or start.get("hand_type") != "right" - or start.get("tag_layout") != "g20_right_19" + or start.get("hand_type") != config.side + or start.get("tag_layout") != config.tag_layout or start.get("source_urdf_sha256") != config.source_urdf_sha256 ): @@ -550,9 +551,9 @@ def run( print( "\n".join( [ - "PASS：G20右手标定、URDF修正和独立复验全部通过。", + f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。", f"正式结果：{config.session_root / 'latest_passed'}", - f"JSON：{session / f'g20_right_{config.serial_number}_calibration.json'}", + f"JSON：{session / summary['artifacts']['json']}", f"URDF：{summary['artifacts']['urdf']}", ] ), @@ -564,7 +565,7 @@ def run( def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description="G20右手一键精密标定") + parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定") parser.add_argument("--config", default=str(_default_product_config())) parser.add_argument("--workspace", default=None) parser.add_argument("--preflight-only", action="store_true") diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py index 2f7960b..df512ba 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py @@ -55,6 +55,43 @@ def _task_tag_id_status(views: Mapping[str, Any]) -> str: ) +def _pnp_wait_status(views: Mapping[str, Any]) -> str: + """Explain why visible Tags have not produced a usable 3-D pose.""" + labels = {"front": "正面", "side": "侧面", "top": "顶部"} + details: list[str] = [] + for name in ("front", "side", "top"): + item = views.get(name, {}) + if not isinstance(item, Mapping) or item.get("pnp_pose_valid"): + continue + parts: list[str] = [] + progress = item.get("pnp_initialization_progress") + if isinstance(progress, Mapping): + parts.append( + "初始化" + f"{int(progress.get('accepted', 0))}/" + f"{int(progress.get('required', 0))}" + ) + counts: dict[str, int] = {} + for field in ("pnp_rejection_counts", "group_pnp_rejection_counts"): + values = item.get(field, {}) + if not isinstance(values, Mapping): + continue + for reason, count in values.items(): + counts[str(reason)] = counts.get(str(reason), 0) + int(count) + if counts: + common = sorted( + counts.items(), key=lambda pair: (-pair[1], pair[0]) + )[:2] + parts.append( + "拒绝=" + ",".join(f"{reason}×{count}" for reason, count in common) + ) + if parts: + details.append(f"{labels[name]} " + "；".join(parts)) + if not details: + return "" + return "PnP：" + " | ".join(details) + "（Tag可见不等于三维位姿有效）" + + @dataclass class ProgressEstimator: started_at: float @@ -202,7 +239,7 @@ def render_progress_zh( ) else: tag_status = ( - f"{visible_tags}/{required_tags} 有效" + f"{visible_tags}/{required_tags} 可见" + (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "") ) task_tag_ids = ( @@ -233,10 +270,23 @@ def render_progress_zh( f"Tag：{tag_status}{task_tag_ids} 相机：{ready_cameras}/3 正常 反馈：{feedback_hz:.1f} Hz", f"质量：有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次", ] - if fit_attempt_limit > 1: + if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start": + pnp_wait = _pnp_wait_status(views) + if pnp_wait: + lines.append(pnp_wait) + fit_retry_cycles = [ + int(cycle) for cycle in active.get("fit_retry_cycles", []) + ] + if fit_attempt > 1 and fit_retry_cycles: + cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles) lines.append( - f"拟合：整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试；" - "每次包含完整四轮双向扫描" + f"拟合：首次拟合定位第 {cycle_text} 轮异常，" + "仅补采该轮双向；其余轮次数据保留" + ) + elif fit_attempt_limit > 1: + lines.append( + f"拟合：任务级硬门限检查（最多允许 " + f"{fit_attempt_limit - 1} 次异常轮补采）" ) resume = status.get("resume", {}) if isinstance(resume, Mapping) and resume.get("used"): @@ -266,6 +316,25 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st for item in views.values() if isinstance(item, Mapping) and item.get("group_pnp_reason") ] if isinstance(views, Mapping) else [] + pnp_diagnostics_present = bool( + group_pnp_reasons + or ( + isinstance(views, Mapping) + and any( + isinstance(item, Mapping) + and any( + item.get(field) + for field in ( + "pnp_rejection_counts", + "group_pnp_rejection_counts", + "pnp_initialization_progress", + "group_missing_candidate_roles", + ) + ) + for item in views.values() + ) + ) + ) if value.startswith("CFG-"): return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。" if value.startswith("CAM-STATUS-202"): @@ -280,6 +349,13 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st "标定节点状态心跳停止更新", "已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。", ) + if "palm_orientation_quality_failed" in value: + return ( + "VAL-QUALITY-501", + "掌部方向校正的可观测轨迹不足", + "保持Tag安装不变；调整标定前手位，使正面Tag 10–13至少三枚" + "在对应运动起始段可见。", + ) if "motor_state_stalled" in value: return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。" if any( @@ -295,9 +371,19 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st if ( ("synchronised" in value or "sweep_start_tag_timeout" in value) and not missing - and group_pnp_reasons + and pnp_diagnostics_present ): - return "CAM-GEOMETRY-201", "Tag可见，但整组PnP候选持续被几何检查拒绝", "不要调整Tag；复制本诊断块给开发者检查PnP候选选择。" + problem = ( + "Tag可见，但三维PnP轨迹在运动中失效" + if value.startswith("synchronised") + and int(active.get("valid_frames", 0) or 0) > 0 + else "Tag可见，但三维PnP位姿初始化未完成" + ) + return ( + "CAM-GEOMETRY-201", + problem, + "不要根据二维码可见性调整Tag；复制累计PnP拒绝原因给开发者。", + ) if missing or "tag" in value or "detection" in value or "synchronised" in value: return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。" if "state" in value and ("timeout" in value or "lost" in value): @@ -308,6 +394,15 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。" if "fit" in value or "trajectory" in value or "axis" in value: return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。" + if ( + "endpoint_zero_offsets' is not defined" in value + or "validated_endpoint_zero_state" in value + ): + return ( + "PUB-ARTIFACT-601", + "已验证零位状态未能完整传递到URDF发布阶段", + "采集与验证数据仍可保留；不要移动相机或Tag，复制诊断块给开发者。", + ) if "validation" in value or "combination" in value or "quality" in value or "zero" in value: return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布；复制本诊断块给开发者。" if "publish" in value or "artifact" in value or "URDF" in value: @@ -338,6 +433,20 @@ def build_failure_report( } if group_pnp_reasons: active["group_pnp_reasons"] = group_pnp_reasons + for field in ( + "pnp_rejection_counts", + "group_pnp_rejection_counts", + "pnp_initialization_progress", + "group_missing_candidate_roles", + "pnp_candidate_diagnostics", + ): + values = { + str(view): item.get(field) + for view, item in views.items() + if isinstance(item, Mapping) and item.get(field) + } + if values: + active[field] = values try: explanation, automatic_action = three_camera_reason_zh( str(status.get("state", "")), reason, active @@ -359,8 +468,43 @@ def build_failure_report( metrics = { "valid_frames": active.get("valid_frames"), "sample": active.get("sample", {}), + "detection_frames": active.get("detection_frames"), + "detection_valid_frames": active.get("detection_valid_frames"), + "detection_rate": active.get("detection_rate"), + "detection_rate_by_view": active.get("detection_rate_by_view", {}), "automatic_retry_count": active.get("automatic_retry_count", 0), } + pnp_diagnostics = { + str(view): { + field: item.get(field) + for field in ( + "group_pnp_reason", + "group_missing_candidate_roles", + "pnp_rejections", + "pnp_rejection_counts", + "group_pnp_rejection_counts", + "pnp_initialization_progress", + "pnp_candidate_diagnostics", + ) + if item.get(field) + } + for view, item in ( + views.items() if isinstance(views, Mapping) else [] + ) + if isinstance(item, Mapping) + and any( + item.get(field) + for field in ( + "group_pnp_reason", + "group_missing_candidate_roles", + "pnp_rejections", + "pnp_rejection_counts", + "group_pnp_rejection_counts", + "pnp_initialization_progress", + "pnp_candidate_diagnostics", + ) + ) + } payload: dict[str, Any] = { "schema_version": 1, "serial_number": config.serial_number, @@ -374,6 +518,7 @@ def build_failure_report( "automatic_action_zh": automatic_action, "suggestion_zh": suggestion, "metrics": metrics, + "pnp_diagnostics": pnp_diagnostics, "camera_state": camera_state, "feedback_hz": status.get("feedback_hz", 0.0), "hashes": { @@ -397,6 +542,12 @@ def build_failure_report( f"详细说明：{explanation}", f"自动处理：{automatic_action}", f"关键指标：{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}", + "PnP诊断：" + + json.dumps( + pnp_diagnostics, + ensure_ascii=False, + separators=(",", ":"), + ), f"相机状态：{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}", f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz", f"配置哈希：{payload['hashes']['product_config_sha256']}", diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py index b1b42e0..9ee5c91 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py @@ -913,11 +913,15 @@ class SquareTagPoseTracker: self.last_candidates_by_role: dict[ str, tuple[SquareTagPose, ...] ] = {} + self.last_candidate_diagnostics_by_role: dict[ + str, dict[str, float | int] + ] = {} self.branch_correction_counts: dict[str, int] = {} def reset(self) -> None: self._previous.clear() self.last_candidates_by_role.clear() + self.last_candidate_diagnostics_by_role.clear() self.branch_correction_counts.clear() def estimate( @@ -938,28 +942,91 @@ class SquareTagPoseTracker: ) except (ValueError, cv2.error): self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } return None, "pnp_solve_failed" if not candidates: self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } return None, "pnp_solve_failed" - usable_candidates: list[SquareTagPose] = [] - for candidate in candidates: + reprojection_candidates = [ + candidate + for candidate in candidates + if candidate.reprojection_error_px + <= self.maximum_reprojection_error_px + ] + candidate_tilts_rad: list[float] = [] + independent_candidates: list[SquareTagPose] = [] + for candidate in reprojection_candidates: normal = Rotation.from_quat( candidate.quaternion_xyzw ).as_matrix()[:, 2] tilt = math.acos( float(np.clip(abs(normal[2]), 0.0, 1.0)) ) - if ( - candidate.reprojection_error_px - <= self.maximum_reprojection_error_px - and tilt <= self.maximum_tag_tilt_rad - ): - usable_candidates.append(candidate) + candidate_tilts_rad.append(float(tilt)) + if tilt <= self.maximum_tag_tilt_rad: + independent_candidates.append(candidate) + + # Candidate generation and candidate selection have different + # contracts. The per-Tag tilt limit protects a pose used without any + # other geometry, but it must not erase a finite, low-reprojection + # IPPE solution before SquareTagGroupPoseTracker can evaluate it + # against the fixed palm reference, the articulated chain and the + # preceding group pose. At a strongly oblique view the planar + # ambiguity is usually smaller, and rejecting both branches at a + # fixed angle caused deterministic mid-sweep holes despite continuous + # image detections. Group tracking therefore receives every + # reprojection-valid candidate; independent tracking below retains the + # original tilt safety gate. self.last_candidates_by_role[str(role)] = tuple( - usable_candidates + reprojection_candidates ) - if not usable_candidates: + diagnostics: dict[str, float | int] = { + "solved_candidate_count": len(candidates), + "reprojection_candidate_count": len(reprojection_candidates), + "independent_tilt_candidate_count": len(independent_candidates), + "minimum_reprojection_error_px": float( + min( + candidate.reprojection_error_px + for candidate in candidates + ) + ), + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + if candidate_tilts_rad: + diagnostics["minimum_candidate_tilt_deg"] = math.degrees( + min(candidate_tilts_rad) + ) + diagnostics["maximum_candidate_tilt_deg"] = math.degrees( + max(candidate_tilts_rad) + ) + self.last_candidate_diagnostics_by_role[str(role)] = diagnostics + if not independent_candidates: return None, "no_pose_within_reprojection_or_tilt_limit" previous_record = self._previous.get(str(role)) @@ -971,7 +1038,7 @@ class SquareTagPoseTracker: previous = previous_pose selected, reason = select_continuous_pose( - usable_candidates, + independent_candidates, previous=previous, maximum_reprojection_error_px=( self.maximum_reprojection_error_px @@ -1180,6 +1247,7 @@ class SquareTagGroupPoseTracker: self._task_reference_relative_poses: dict[ tuple[str, str], tuple[Rotation, np.ndarray] ] = {} + self.last_missing_roles: tuple[str, ...] = () def reset(self, *, preserve_task_reference: bool = False) -> None: self._previous.clear() @@ -1190,6 +1258,7 @@ class SquareTagGroupPoseTracker: self.branch_correction_counts.clear() self._decreasing_relative_rotations.clear() self._coupled_reference_rotations.clear() + self.last_missing_roles = () if not preserve_task_reference: self._task_reference_relative_poses.clear() @@ -1369,8 +1438,14 @@ class SquareTagGroupPoseTracker: tuple(candidates_by_role.get(role, ())) for role in self.roles ] - if any(not candidates for candidates in candidate_lists): + self.last_missing_roles = tuple( + role + for role, candidates in zip(self.roles, candidate_lists) + if not candidates + ) + if self.last_missing_roles: return None, "group_missing_pose_candidates" + self.last_missing_roles = () combinations = [ dict(zip(self.roles, combination)) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py index 46febbe..cf5d591 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py @@ -1,4 +1,10 @@ -"""Immutable G20 right-hand product identity and one-command preflight.""" +"""Immutable product identity and shared calibration preflight. + +Model-specific kinematics live in registered calibration contracts. The +loader itself only verifies that the selected contract, Tag configuration, +cameras and immutable artifacts agree, so adding another hand does not +require another copy of the one-command runner. +""" from __future__ import annotations @@ -11,11 +17,113 @@ from typing import Any, Mapping import yaml from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics +from .full_hand import ( + G20_RIGHT_19_LAYOUT, + HandCalibrationProfile, + get_hand_calibration_profile, +) +from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile VIEWS = ("front", "side", "top") +@dataclass(frozen=True) +class ProductCalibrationContract: + """Declarative boundary between one hand model and the shared engine.""" + + model: str + side: str + layout_id: str + profile: HandCalibrationProfile + zero_profile: ZeroCalibrationProfile + + @property + def required_tag_ids(self) -> frozenset[int]: + return frozenset( + int(tag_id) + for tags in self.profile.view_tags.values() + for tag_id in tags.values() + ) + + @property + def views(self) -> tuple[str, ...]: + return tuple(self.profile.view_tags) + + +_PRODUCT_CONTRACTS: dict[ + tuple[str, str, str], ProductCalibrationContract +] = {} + + +def register_product_calibration_contract( + contract: ProductCalibrationContract, +) -> None: + """Register one model/side/layout without modifying shared workflow code.""" + model = str(contract.model).strip().upper() + side = str(contract.side).strip().lower() + layout = str(contract.layout_id).strip().lower() + if not model or side not in {"left", "right"} or not layout: + raise ValueError("product calibration contract identity is invalid") + if contract.profile.side != side: + raise ValueError("product contract side differs from hand profile") + if contract.profile.layout_id.lower() != layout: + raise ValueError("product contract layout differs from hand profile") + if contract.zero_profile.hand != contract.profile: + raise ValueError("zero-calibration profile differs from hand profile") + if len(contract.profile.baseline_command) != contract.profile.command_count: + raise ValueError("profile baseline and command names differ in length") + if not contract.required_tag_ids: + raise ValueError("product contract must declare at least one Tag") + key = (model, side, layout) + existing = _PRODUCT_CONTRACTS.get(key) + if existing is not None and existing != contract: + raise ValueError(f"product calibration contract already registered: {key}") + _PRODUCT_CONTRACTS[key] = contract + + +def get_product_calibration_contract( + model: str, side: str, layout_id: str +) -> ProductCalibrationContract: + key = ( + str(model).strip().upper(), + str(side).strip().lower(), + str(layout_id).strip().lower(), + ) + try: + return _PRODUCT_CONTRACTS[key] + except KeyError as error: + supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS)) + raise ValueError( + f"unsupported calibration product {key}; registered={supported}" + ) from error + + +register_product_calibration_contract( + ProductCalibrationContract( + model="G20", + side="right", + layout_id=G20_RIGHT_19_LAYOUT, + profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), + zero_profile=get_zero_calibration_profile( + "right", G20_RIGHT_19_LAYOUT + ), + ) +) +for _legacy_side in ("left", "right"): + register_product_calibration_contract( + ProductCalibrationContract( + model="G20", + side=_legacy_side, + layout_id="legacy_11", + profile=get_hand_calibration_profile(_legacy_side, "legacy_11"), + zero_profile=get_zero_calibration_profile( + _legacy_side, "legacy_11" + ), + ) + ) + + def sha256_file(path: str | Path) -> str: digest = hashlib.sha256() with Path(path).open("rb") as stream: @@ -104,6 +212,10 @@ def _custom_pnp_tag_sizes_m_by_id( class ProductConfig: path: Path workspace: Path + model: str + side: str + tag_layout: str + calibration_contract: ProductCalibrationContract serial_number: str can_interface: str source_urdf: Path @@ -130,7 +242,7 @@ def load_product_config( workspace: str | Path | None = None, check_can: bool = True, ) -> ProductConfig: - """Load and fully verify the fixed G20_RIGHT_001 product configuration.""" + """Load and verify a product against its registered calibration contract.""" source = Path(path).expanduser().resolve() if not source.is_file(): raise ValueError(f"product config does not exist: {source}") @@ -142,8 +254,17 @@ def load_product_config( serial = str(raw.get("serial_number", "")) if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None: raise ValueError("serial_number is invalid") - if raw.get("model") != "G20" or raw.get("side") != "right": - raise ValueError("product config must describe the G20 right hand") + model = str(raw.get("model", "")).strip().upper() + side = str(raw.get("side", "")).strip().lower() + # Keep the deployed G20 schema compatible while making the layout an + # explicit product choice for all new configurations. + layout = str( + raw.get( + "tag_layout", + G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "", + ) + ).strip().lower() + contract = get_product_calibration_contract(model, side, layout) can_interface = str(raw.get("can_interface", "")).strip() if not can_interface: raise ValueError("can_interface is required") @@ -197,16 +318,13 @@ def load_product_config( if actual != expected: raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}") - expected_tag_ids = set(range(19)) + expected_tag_ids = set(contract.required_tag_ids) tag_sizes = _tag_sizes_m_by_id(tag_config) if set(tag_sizes) != expected_tag_ids: raise ValueError( - "g20_right_19 Tag config must contain exactly IDs 0 through 18" - ) - expected_tag_sizes = {tag_id: 0.016 for tag_id in expected_tag_ids} - if tag_sizes != expected_tag_sizes: - raise ValueError( - "g20_right_19 black-code Tag sizes must be 16 mm for every ID" + f"{model}/{side}/{layout} Tag IDs differ from the registered " + f"profile: expected={sorted(expected_tag_ids)} " + f"actual={sorted(tag_sizes)}" ) if _custom_pnp_tag_sizes_m_by_id( calibration_config, expected_tag_ids @@ -216,12 +334,17 @@ def load_product_config( ) camera_raw = _mapping(raw.get("cameras"), "cameras") - if set(camera_raw) != set(VIEWS): - raise ValueError("cameras must contain front/side/top") + required_views = tuple(contract.views) + if set(required_views) != set(VIEWS): + raise ValueError( + "the current shared engine requires front/side/top views" + ) + if set(camera_raw) != set(required_views): + raise ValueError(f"cameras must contain {'/'.join(required_views)}") loaded_extrinsics = load_three_camera_extrinsics(extrinsics) cameras: dict[str, dict[str, str]] = {} serials: set[str] = set() - for view in VIEWS: + for view in required_views: item = _mapping(camera_raw[view], f"cameras.{view}") camera_serial = str(item.get("serial_number", "")) info_path = _resolve_path( @@ -259,6 +382,10 @@ def load_product_config( return ProductConfig( path=source, workspace=root, + model=model, + side=side, + tag_layout=layout, + calibration_contract=contract, serial_number=serial, can_interface=can_interface, source_urdf=source_urdf, diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py index 597d5eb..0f9b8df 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py @@ -24,7 +24,7 @@ from .full_hand import ( from .product import ProductConfig, sha256_file from .storage import atomic_write_json from .urdf_zero import ( - RIGHT_19_FLEXION_ENDPOINT_JOINTS, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, get_zero_calibration_profile, ) @@ -41,6 +41,7 @@ ACTIVE_ZERO_JOINTS = frozenset( RETAINED_ACTIVE_ZERO_JOINTS = frozenset( get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints ) - ACTIVE_ZERO_JOINTS +SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9 def _load_json(path: Path) -> dict[str, Any]: @@ -89,7 +90,7 @@ def _mask_active_origin_rpy_fields(text: str) -> str: def _mask_endpoint_coordinate_fields(text: str) -> str: - """Mask only limit/mimic fields induced by flexion zero coordinates.""" + """Mask only limit/mimic fields induced by endpoint zero coordinates.""" pattern = re.compile( r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", re.DOTALL, @@ -98,7 +99,7 @@ def _mask_endpoint_coordinate_fields(text: str) -> str: def replace(match: re.Match[str]) -> str: block = match.group(0) name = match.group("name") - if name in RIGHT_19_FLEXION_ENDPOINT_JOINTS: + if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS: return re.sub( r"(]*\bupper\s*=\s*)([\"'])[^\"']*\2", r"\1\2__CALIBRATED_UPPER__\2", @@ -110,7 +111,10 @@ def _mask_endpoint_coordinate_fields(text: str) -> str: block, re.DOTALL, ) - if mimic is not None and mimic.group("source") in RIGHT_19_FLEXION_ENDPOINT_JOINTS: + if ( + mimic is not None + and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + ): return re.sub( r"(]*\boffset\s*=\s*)([\"'])[^\"']*\2", r"\1\2__CALIBRATED_MIMIC_OFFSET__\2", @@ -145,7 +149,7 @@ def _verify_expected_origin_offsets( if set(offsets) != ACTIVE_ZERO_JOINTS or any( not math.isfinite(value) for value in offsets.values() ): - raise ValueError("expected offsets must contain 12 finite static-zero values") + raise ValueError("expected offsets must contain all finite active static-zero values") before = _joint_elements(source) after = _joint_elements(corrected) maximum_rotation_error = 0.0 @@ -186,7 +190,7 @@ def _verify_expected_origin_offsets( # so comparing that file with the published JSON necessarily permits half # of one last-place unit. This is about 2.9e-7 degrees and is far below # any calibration or URDF numerical significance. - if maximum_rotation_error > 5.1e-9: + if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD: raise ValueError( "corrected URDF origin.rpy does not match the published zero offsets" ) @@ -245,7 +249,13 @@ def verify_corrected_urdf( original_limit = source_joints[name].find("limit") corrected_limit = corrected_joints[name].find("limit") expected_upper = float(original_limit.get("upper")) - offset - if abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-10: + # Schema v4 stores the corresponding zero at eight decimal + # places, while the URDF is written from the full-precision solve. + # Match the half-last-place tolerance used for origin rotations. + if ( + abs(float(corrected_limit.get("upper")) - expected_upper) + > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + ): raise ValueError(f"corrected URDF has invalid {name} upper limit") for name in PASSIVE_JOINTS: original_mimic = source_joints[name].find("mimic") @@ -253,9 +263,20 @@ def verify_corrected_urdf( if original_mimic is None or corrected_mimic is None: continue source_name = str(original_mimic.get("joint")) + multiplier = float(original_mimic.get("multiplier", "1")) expected = float(original_mimic.get("offset", "0")) - expected += float(original_mimic.get("multiplier", "1")) * endpoint_offsets.get(source_name, 0.0) - if abs(float(corrected_mimic.get("offset", "0")) - expected) > 1.0e-10: + expected += multiplier * endpoint_offsets.get(source_name, 0.0) + # The active endpoint offset comes from schema-v4 JSON rounded to + # eight decimal places, while the URDF mimic was written from the + # full-precision solve. Propagate exactly the same accepted + # quantization through the mimic multiplier; retain a much + # smaller allowance for XML decimal formatting itself. + tolerance = ( + abs(multiplier) + * SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + + 1.0e-12 + ) + if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance: raise ValueError(f"corrected URDF has invalid {name} mimic offset") return tuple(sorted(changed)) @@ -284,55 +305,62 @@ def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]: def validate_runtime_curves_against_urdf_limits( - payload: Mapping[str, Any], source_urdf: str | Path + payload: Mapping[str, Any], runtime_urdf: str | Path ) -> None: - """Reject a runtime curve whose commanded q leaves a CAD safety limit.""" + """Reject a curve whose q leaves its runtime URDF coordinate limits. + + Curves in schema v4 are expressed in the corrected URDF joint coordinate, + not in the source-CAD coordinate. Endpoint zero calibration can therefore + move a corrected coordinate limit while preserving the same physical CAD + endpoint; callers publishing a calibrated pair must pass that corrected + URDF here. + """ validate_compact_payload(payload) - source_joints = _joint_elements(source_urdf) + runtime_joints = _joint_elements(runtime_urdf) for name, calibration in payload["joints"].items(): - joint = source_joints.get(str(name)) + joint = runtime_joints.get(str(name)) if joint is None: - raise ValueError(f"source URDF is missing runtime joint {name}") + raise ValueError(f"runtime URDF is missing joint {name}") limit = joint.find("limit") if limit is None or limit.get("lower") is None or limit.get("upper") is None: - raise ValueError(f"source URDF joint {name} has no finite position limit") + raise ValueError(f"runtime URDF joint {name} has no finite position limit") lower = float(limit.get("lower")) upper = float(limit.get("upper")) if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: - raise ValueError(f"source URDF joint {name} has invalid position limits") + raise ValueError(f"runtime URDF joint {name} has invalid position limits") curve = np.asarray(calibration["angle_rad"], dtype=float) minimum = float(np.min(curve)) maximum = float(np.max(curve)) tolerance = 1.0e-7 if minimum < lower - tolerance or maximum > upper + tolerance: raise ValueError( - f"runtime curve exceeds source URDF limit for {name}: " + f"runtime curve exceeds runtime URDF limit for {name}: " f"[{minimum:.9g}, {maximum:.9g}] not within " f"[{lower:.9g}, {upper:.9g}]" ) def clamp_compact_payload_to_urdf_limits( - payload: Mapping[str, Any], source_urdf: str | Path + payload: Mapping[str, Any], runtime_urdf: str | Path ) -> tuple[dict[str, Any], dict[str, int]]: - """Return a schema-preserving runtime payload bounded by CAD limits.""" + """Return a schema-preserving payload bounded in its runtime coordinates.""" result = copy.deepcopy(dict(payload)) validate_compact_payload(result) - source_joints = _joint_elements(source_urdf) + runtime_joints = _joint_elements(runtime_urdf) clipped_by_joint: dict[str, int] = {} for name, calibration in result["joints"].items(): - joint = source_joints.get(str(name)) + joint = runtime_joints.get(str(name)) limit = None if joint is None else joint.find("limit") if ( limit is None or limit.get("lower") is None or limit.get("upper") is None ): - raise ValueError(f"source URDF joint {name} has no finite position limit") + raise ValueError(f"runtime URDF joint {name} has no finite position limit") lower = float(limit.get("lower")) upper = float(limit.get("upper")) if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: - raise ValueError(f"source URDF joint {name} has invalid position limits") + raise ValueError(f"runtime URDF joint {name} has invalid position limits") source = np.asarray(calibration["angle_rad"], dtype=float) bounded = np.clip(source, lower, upper) count = int(np.count_nonzero(bounded != source)) @@ -429,7 +457,10 @@ def compare_session_offsets( ) -> dict[str, float]: left = active_offsets(first) right = active_offsets(second) - differences = {name: abs(left[name] - right[name]) for name in sorted(left)} + differences = { + name: abs(left[name] - right[name]) + for name in sorted(left) + } failed = {name: value for name, value in differences.items() if value > maximum_difference_rad} if failed: details = ", ".join( @@ -549,14 +580,15 @@ def finalize_session_artifacts( raise ValueError("node status is missing combination validation") _verify_combination_validation(combination) offsets = active_offsets(payload) + endpoint_offsets = { + name: offsets[name] + for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + } changed_joints = verify_corrected_urdf( config.source_urdf, paths["urdf"], expected_offsets_rad=offsets, - endpoint_anchored_offsets_rad={ - name: offsets[name] - for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS - }, + endpoint_anchored_offsets_rad=endpoint_offsets, ) payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( payload, paths["urdf"] @@ -628,8 +660,13 @@ def finalize_session_artifacts( config.source_urdf, paths["urdf"], expected_offsets_rad=offsets, + endpoint_anchored_offsets_rad=endpoint_offsets, ) - validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) + # Revalidate the same coordinate contract immediately before publication: + # verify_corrected_urdf proves that corrected endpoint limits map back to + # the original physical CAD endpoints, while the runtime curves must stay + # inside those corrected-coordinate limits. + validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) final_mesh_hashes = { name: sha256_file(path) for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py index 0ad597f..9761db9 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py @@ -3,7 +3,7 @@ from __future__ import annotations import re -from typing import Any, Mapping +from typing import Any, Mapping, Sequence STATE_NAMES_ZH = { @@ -115,10 +115,13 @@ def _task_text(active: Mapping[str, Any]) -> str: ) fit_attempt = int(active.get("fit_attempt", 1)) if fit_attempt > 1: - task += ( - f"（整关节自动重采第{fit_attempt}/" - f"{active.get('fit_attempt_limit', '?')}次）" - ) + retry_cycles = active.get("fit_retry_cycles", []) + if retry_cycles: + task += "（补采异常轮" + "/".join( + str(cycle) for cycle in retry_cycles + ) + "）" + else: + task += f"（拟合补采第{fit_attempt}次）" return task @@ -240,14 +243,87 @@ def three_camera_reason_zh( if reason == "synchronised_tag_state_timeout": group_reasons = active.get("group_pnp_reasons", {}) if isinstance(group_reasons, Mapping) and group_reasons: - reason_text = "、".join( - f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}" - for view, value in group_reasons.items() + tag_rejections = active.get("pnp_rejection_counts", {}) + group_rejections = active.get( + "group_pnp_rejection_counts", {} ) + missing_roles = active.get( + "group_missing_candidate_roles", {} + ) + candidate_diagnostics = active.get( + "pnp_candidate_diagnostics", {} + ) + view_details: list[str] = [] + for view, value in group_reasons.items(): + view_name = str(view) + parts = [str(value)] + missing = ( + missing_roles.get(view_name, ()) + if isinstance(missing_roles, Mapping) + else () + ) + if isinstance(missing, Sequence) and not isinstance( + missing, (str, bytes) + ) and missing: + parts.append( + "缺候选=" + ",".join(str(role) for role in missing) + ) + counts: dict[str, int] = {} + for source in (tag_rejections, group_rejections): + values = ( + source.get(view_name) + if isinstance(source, Mapping) + else None + ) + if isinstance(values, Mapping): + for name, count in values.items(): + counts[str(name)] = counts.get(str(name), 0) + int( + count + ) + if counts: + common = sorted( + counts.items(), key=lambda pair: (-pair[1], pair[0]) + )[:3] + parts.append( + "累计拒绝=" + + ",".join( + f"{name}×{count}" for name, count in common + ) + ) + view_candidates = ( + candidate_diagnostics.get(view_name, {}) + if isinstance(candidate_diagnostics, Mapping) + else {} + ) + if isinstance(view_candidates, Mapping) and missing: + summaries: list[str] = [] + for role in missing: + diagnostic = view_candidates.get(str(role), {}) + if not isinstance(diagnostic, Mapping): + continue + summaries.append( + f"{role}(solve=" + f"{int(diagnostic.get('solved_candidate_count', 0))}," + "reproj=" + f"{int(diagnostic.get('reprojection_candidate_count', 0))}," + "tilt=" + f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})" + ) + if summaries: + parts.append("候选统计=" + ",".join(summaries)) + view_details.append( + f"{VIEW_NAMES_ZH.get(view_name, view_name)}=" + + "；".join(parts) + ) + reason_text = "、".join(view_details) return ( - f"{detail_prefix}Tag仍可见且反馈正常，但连续图像帧被整组PnP几何检查拒绝" + f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，" + "后续连续图像帧" + "被整组PnP几何检查拒绝" f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。", - "不要调整或反复粘贴Tag；保留当前会话并复制诊断块给开发者检查PnP分支逻辑。", + "不要调整或反复粘贴Tag；保留当前会话中的" + "group_pnp_candidate_event，" + "按缺失角色的候选统计检查PnP分支逻辑。", ) return ( f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" @@ -273,15 +349,81 @@ def three_camera_reason_zh( ) if reason == "sweep_start_tag_timeout": group_reasons = active.get("group_pnp_reasons", {}) - if isinstance(group_reasons, Mapping) and group_reasons: - reason_text = "、".join( - f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}" - for view, value in group_reasons.items() + progress_by_view = active.get("pnp_initialization_progress", {}) + tag_rejections = active.get("pnp_rejection_counts", {}) + group_rejections = active.get("group_pnp_rejection_counts", {}) + if any( + isinstance(value, Mapping) and bool(value) + for value in ( + group_reasons, + progress_by_view, + tag_rejections, + group_rejections, ) + ): + details: list[str] = [] + views = set() + for value in ( + group_reasons, + progress_by_view, + tag_rejections, + group_rejections, + ): + if isinstance(value, Mapping): + views.update(str(view) for view in value) + for view in sorted(views): + parts: list[str] = [] + progress = ( + progress_by_view.get(view) + if isinstance(progress_by_view, Mapping) + else None + ) + if isinstance(progress, Mapping): + parts.append( + "初始化" + f"{int(progress.get('accepted', 0))}/" + f"{int(progress.get('required', 0))}" + ) + counts: dict[str, int] = {} + for source in (tag_rejections, group_rejections): + values = ( + source.get(view) + if isinstance(source, Mapping) + else None + ) + if isinstance(values, Mapping): + for name, count in values.items(): + counts[str(name)] = ( + counts.get(str(name), 0) + int(count) + ) + if counts: + common = sorted( + counts.items(), key=lambda pair: (-pair[1], pair[0]) + )[:3] + parts.append( + "累计拒绝=" + + ",".join( + f"{name}×{count}" for name, count in common + ) + ) + latest = ( + group_reasons.get(view) + if isinstance(group_reasons, Mapping) + else None + ) + if latest and not str(latest).startswith( + "group_initializing:" + ): + parts.append(f"最后状态={latest}") + if parts: + details.append( + f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts) + ) + reason_text = "、".join(details) or "未形成完整初始化窗口" return ( - "被测电机已经到达扫描起点，所需Tag也可见，但整组PnP初始化持续" - f"拒绝候选（{reason_text}），因此没有生成同步端点帧。", - "不要重新粘贴可见Tag；保留当前会话并复制诊断块给开发者检查PnP安装先验。", + "被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化" + f"没有完成（{reason_text}），因此没有生成同步端点帧。", + "不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。", ) return ( "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的" @@ -308,6 +450,19 @@ def three_camera_reason_zh( "随机复测位置没有采集到足够的同步有效Tag帧。", "检查当前机位Tag可见性后调用resume。", ) + if reason == "palm_orientation_quality_failed": + failures = active.get("failures", []) + detail = ( + str(failures[0].get("reason", "方向观测不足")) + if failures + else "方向观测不足" + ) + return ( + "掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定：" + + detail, + "保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段" + "至少可见15°行程后重新标定。", + ) if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}: metric_names = { "plane_rms_mm": "平面拟合RMS", @@ -327,8 +482,10 @@ def three_camera_reason_zh( "axis_plane_rms_mm": "三维圆轴向RMS", "axis_radial_rms_mm": "三维圆半径RMS", "axis_pose_line_rms_mm": "姿态轨迹轴线RMS", + "axis_line_cycle_rms_mm": "四轮轴线位置RMS", "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角", "axis_cycle_difference_deg": "各轮转轴方向极差", + "cross_view_roll_curve": "正面/侧面关节角曲线差异RMS", "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差", "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS", "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95", @@ -353,6 +510,7 @@ def three_camera_reason_zh( "axis_plane_rms_mm": "mm", "axis_radial_rms_mm": "mm", "axis_pose_line_rms_mm": "mm", + "axis_line_cycle_rms_mm": "mm", "rotation_circle_axis_difference_deg": "°", "axis_cycle_difference_deg": "°", "third_cycle_axis_holdout_deg": "°", @@ -360,6 +518,7 @@ def three_camera_reason_zh( "third_cycle_trajectory_p95_deg": "°", "state_image_sync_p95_ms": "ms", "tag_valid_rate_percent": "%", + "cross_view_roll_curve": "°", } details: list[str] = [] for failure in active.get("failures", []): @@ -402,8 +561,19 @@ def three_camera_reason_zh( for failure in active.get("failures", []) ) if reason == "joint_fit_systematic_failure": + cross_view_systematic = any( + failure.get("classification") + in { + "stable_cross_view_installation_or_model_bias", + "stable_cross_view_direction_conflict", + } + for failure in active.get("failures", []) + ) suggestion = ( - "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。" + "四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，" + "继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。" + if cross_view_systematic + else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。" "请直接复制诊断块给开发者，不要放宽门限。" ) else: @@ -543,6 +713,18 @@ def three_camera_reason_zh( return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。" if reason == "quality_failed": return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。" + if reason.startswith("validated_endpoint_zero_state"): + return ( + "轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与" + "已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。", + "不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。", + ) + if reason.startswith("PUB-ARTIFACT-601:"): + return ( + "标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前" + "发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。", + "不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。", + ) if reason == "combination_pose_prediction_failed": return ( "单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。", diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py index 89726ce..f0404e2 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py @@ -57,6 +57,7 @@ from .full_hand import ( THREE_CAMERA_BASELINE_COMMAND, HandCalibrationProfile, JointCurveFit, + PalmAxisObserver, SweepSpec, build_calibration_motion_command, build_calibration_preparation_waypoints, @@ -67,6 +68,7 @@ from .full_hand import ( canonical_zero_direction, clamp_runtime_fits_to_urdf_limits, compare_cross_view_roll_curves, + cross_view_roll_diagnostic_metrics, derive_mimic_passive_fits, fit_joint_image_curve, get_hand_calibration_profile, @@ -77,24 +79,36 @@ from .pnp import ( SquareTagPose, SquareTagPoseTracker, ) +from .product import get_product_calibration_contract from .storage import append_jsonl, append_jsonl_many, atomic_write_json from .three_camera_diagnostics import render_three_camera_status_text_zh from .urdf_zero import ( LEFT_ZERO_PROFILE, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, JointAxisMeasurement, + PalmOrientationMeasurement, UrdfKinematicModel, ZeroCalibrationProfile, ZeroSolveResult, + anchor_right_19_mechanical_endpoint_curves, + axis_line_cycle_rms_m, + axis_line_uses_depth_free_interpretation_plane, baseline_hysteresis_by_cycle_rad, circle_direction_is_constrained, + cross_view_side_line_source, fit_joint_axis_measurement, + fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, - derive_right_19_flexion_endpoint_offsets, + derive_right_19_mechanical_endpoint_offsets, joint_curve_holdout_errors, + maximum_axis_line_cycle_spread_m, measure_joint_curve_observation, + refit_axis_line_group_with_shared_radius, + select_cross_view_roll_direction_source, solve_urdf_zero_offsets, get_zero_calibration_profile, write_zero_corrected_urdf, + with_depth_free_axis_projection, ) @@ -252,10 +266,80 @@ def _sweep_views( return tuple(views or [spec.view]) +def _palm_axis_observer_for_sweep( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> PalmAxisObserver | None: + """Return the one non-blocking palm observer attached to this task/view.""" + matches = tuple( + observer + for observer in profile.palm_axis_observers + if observer.task_name == spec.key and observer.view == str(view) + ) + if len(matches) > 1: + raise ValueError( + f"multiple palm-axis observers configured for {spec.key}:{view}" + ) + return matches[0] if matches else None + + +def _palm_axis_observer_schema( + profile: HandCalibrationProfile, +) -> list[dict[str, Any]]: + return [ + { + "source_name": item.source_name, + "task_name": item.task_name, + "view": item.view, + "parent_role": item.parent_role, + "child_role": item.child_role, + "model_joint": item.model_joint, + "motor_index": int(item.motor_index), + } + for item in profile.palm_axis_observers + ] + + +def _palm_axis_resume_policy( + profile: HandCalibrationProfile, + session_start: Mapping[str, Any], +) -> tuple[bool, tuple[str, ...]]: + """Validate optional palm-axis checkpoint data when a model uses it.""" + capability = "palm_axis_side_channel_v1" + previous = {str(value) for value in session_start.get("capabilities", [])} + if not profile.palm_axis_observers and capability not in profile.capabilities: + return True, () + required_previous = set(profile.capabilities) - {capability} + if not required_previous.issubset(previous): + raise ValueError("resume checkpoint lacks required capabilities") + if capability in previous: + if session_start.get("palm_axis_observers") != ( + _palm_axis_observer_schema(profile) + ): + raise ValueError("resume palm-axis observer schema differs") + return True, () + return False, tuple( + observer.task_name for observer in profile.palm_axis_observers + ) + + def _node_profile(node: Any) -> HandCalibrationProfile: return getattr(node, "profile", LEFT_HAND_PROFILE) +def _command_names(node: Any) -> tuple[str, ...]: + profile = _node_profile(node) + names = tuple(getattr(profile, "command_names", ()) or COMMAND_NAMES) + if not names or len(set(names)) != len(names): + raise ValueError("product command names must be non-empty and unique") + return names + + +def _command_count(node: Any) -> int: + return len(_command_names(node)) + + def _sweep_joints_for_view( profile: HandCalibrationProfile, spec: SweepSpec, @@ -332,6 +416,19 @@ def _fit_retry_joint_names( selected: set[str] = set() validation_sources = profile.axis_validation_sources or {} for failure in failures: + quality_sources = failure.get("quality_source_joints", ()) + if ( + isinstance(quality_sources, Sequence) + and not isinstance(quality_sources, (str, bytes)) + ): + selected_sources = { + str(source) + for source in quality_sources + if str(source) in spec.joints + } + if selected_sources: + selected.update(selected_sources) + continue name = str(failure.get("joint", "")) if str(failure.get("metric", "")) == "cross_view_roll_curve": name = str(validation_sources.get(name, name)) @@ -392,6 +489,89 @@ def _isolated_axis_cycle_outliers( return {best_excluded} +def _isolated_axis_line_cycle_outliers( + measurements: Sequence[JointAxisMeasurement], limit_m: float +) -> set[int]: + """Return one cycle whose removal restores line-position repeatability. + + Four independently fitted near-parallel lines can contain one bad PnP + branch or one mechanically unsettled pass. When three cycles form a + clearly tighter cluster below the unchanged formal RMS limit, reacquire + only the excluded cycle. Ambiguous gradual drift still returns no + outlier and therefore keeps the conservative full four-cycle retry. + """ + if len(measurements) < 4 or float(limit_m) <= 0.0: + return set() + full_rms = axis_line_cycle_rms_m(measurements) + if full_rms <= float(limit_m): + return set() + candidates: list[tuple[float, int]] = [] + for excluded in range(len(measurements)): + inliers = [ + measurement + for index, measurement in enumerate(measurements) + if index != excluded + ] + inlier_rms = axis_line_cycle_rms_m(inliers) + if inlier_rms <= float(limit_m): + candidates.append((inlier_rms, excluded)) + if not candidates: + return set() + candidates.sort() + best_rms, best_excluded = candidates[0] + # Require a strong three-cycle cluster and a unique choice. This rejects + # the common gradual-drift case where excluding either endpoint happens + # to move a marginal RMS just below the gate. + if best_rms > 0.75 * float(limit_m): + return set() + if ( + len(candidates) > 1 + and candidates[1][0] - best_rms <= 0.10 * float(limit_m) + ): + return set() + return {best_excluded} + + +def _preserve_pnp_task_reference_for_sweep( + item: SweepItem, *, is_fit_retry: bool, has_precheck_anchor: bool +) -> bool: + """Keep the accepted IPPE branch after precheck and across retries.""" + return bool( + not item.precheck + and ( + item.cycle > 0 + or bool(is_fit_retry) + or bool(has_precheck_anchor) + ) + ) + + +def _requires_pnp_tracker_reset_for_sweep( + profile: HandCalibrationProfile, + item: SweepItem, + *, + is_fit_retry: bool, +) -> bool: + """Reset G20-right PnP once per task, not once per formal cycle. + + The low-speed decreasing precheck establishes a branch with a static + endpoint window. Clearing frame-to-frame state at every later cycle made + four measurements of one rigid installation depend on four independent + planar-PnP initializations. Keep the tracker continuous for the whole + normal task transaction; a bounded retry remains a deliberately fresh + initialization while retaining the accepted task reference. + """ + if item.direction != DIRECTION_DECREASING: + return False + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return True + if is_fit_retry: + return True + if profile.supports("precheck_sweeps"): + return bool(item.precheck) + return not item.precheck and item.cycle == 0 + + def _fixed_base_role(view: str) -> str: return { "front": "front_base", @@ -487,6 +667,63 @@ def _frames_cover_sweep_joints( ) +def _records_have_formal_sweep_coverage( + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + joint_names: Sequence[str], + *, + repetitions: int, + minimum_bins: int, + maximum_bin_gap: int, +) -> bool: + """Return whether every persisted direction has formal bin coverage.""" + expected_groups = { + (cycle, direction) + for cycle in range(int(repetitions)) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + } + if not joint_names or not expected_groups: + return False + for joint_name in joint_names: + grouped: dict[tuple[int, str], set[int]] = {} + for record in records_by_joint.get(joint_name, ()): + direction = str(record.get("direction", "")) + if direction not in { + DIRECTION_DECREASING, + DIRECTION_INCREASING, + }: + continue + command = int( + np.clip( + np.rint( + float( + record.get( + "command_u8", + record.get("feedback_u8", -1), + ) + ) + ), + 0, + 255, + ) + ) + grouped.setdefault( + (int(record.get("cycle", -1)), direction), set() + ).add(command) + if not expected_groups.issubset(grouped): + return False + for key in expected_groups: + commands = sorted(grouped[key]) + if ( + not commands + or commands[0] != 0 + or commands[-1] != 255 + or len(commands) < int(minimum_bins) + or max(np.diff(commands), default=0) > int(maximum_bin_gap) + ): + return False + return True + + def _frames_cover_sweep_motion( frames: Sequence[Any], spec: SweepSpec, @@ -716,6 +953,17 @@ def _unresolved_fit_failure_tasks( # cross-cycle and holdout gates, so the old rejection itself # must not force another eight-direction scan. return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric == "cross_view_roll_curve" + and joint_name in (profile.axis_validation_sources or {}) + ): + # The side roll alias is a validation-only observation in the + # current product model. Its raw samples are still + # revalidated through side-line radial/cycle/holdout gates, + # but a historical front/side curve-shape warning must not + # make a complete task look unfinished on every restart. + return True return metric in { "rotation_circle_axis_difference_deg", "axis_plane_rms_mm", @@ -752,7 +1000,9 @@ def resumable_completed_task_prefix( """ rows = _normalise_legacy_split_roll_resume_rows(profile, rows) samples = _latest_resume_rows( - rows, kind="sample", minimum_sweep_bins=minimum_sweep_bins + rows, + kind="sample", + minimum_sweep_bins=minimum_sweep_bins, ) baselines = _latest_resume_rows(rows, kind="baseline_hold_sample") checkpoints = _latest_resume_rows(rows, kind="steady_command_sample") @@ -852,6 +1102,21 @@ def resumable_completed_task_prefix( -1, ) ) + reusable.extend( + dict(row) + for row in rows + if row.get("kind") == "palm_axis_sample" + and str(row.get("task_name", "")) in completed_set + and int(row.get("attempt", 1)) + == latest_task_attempt.get( + ( + str(row.get("task_name", "")), + int(row.get("cycle", -1)), + str(row.get("direction", "")), + ), + -1, + ) + ) reusable.sort( key=lambda row: ( profile.sweep_specs.index( @@ -935,6 +1200,8 @@ def _fit_failure_is_systematic( """Identify a repeatable model conflict that more motion cannot repair.""" if not failures or int(repetitions) <= 1: return False + if all(bool(item.get("systematic", False)) for item in failures): + return True if any( str(item.get("metric", "")) != "rotation_circle_axis_difference_deg" @@ -961,6 +1228,170 @@ def _fit_failure_is_systematic( ) +def _cross_view_curve_failure( + primary_name: str, + validation_name: str, + primary: JointCurveFit, + validation: JointCurveFit, + *, + scope: str, + maximum_rms_difference_rad: float, + maximum_branch_gap_difference_rad: float, + allow_projection_scale: bool = False, + maximum_projection_scale_ratio: float = 1.5, + cycle_fits: Sequence[tuple[JointCurveFit, JointCurveFit]] = (), +) -> dict[str, Any] | None: + """Return a localized, retry-aware cross-view failure. + + A stable offset repeated in every cycle is an installation/model conflict; + reacquiring the same four motions cannot change it. A single bad cycle is + instead localized so only that side-view cycle is reacquired. + """ + try: + compare_cross_view_roll_curves( + primary, + validation, + maximum_rms_difference_rad=maximum_rms_difference_rad, + maximum_branch_gap_difference_rad=( + maximum_branch_gap_difference_rad + ), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=( + maximum_projection_scale_ratio + ), + ) + return None + except ValueError as error: + reason = str(error) + + metrics: dict[str, float] = {} + if reason != "cross_view_roll_curve_direction_disagrees": + metrics = compare_cross_view_roll_curves( + primary, + validation, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=float("inf"), + ) + failure: dict[str, Any] = { + "joint": primary_name, + "metric": "cross_view_roll_curve", + "reason": reason, + "scope": scope, + "quality_source_joints": [validation_name], + } + metric_key: str | None = None + metric_limit: float | None = None + metric_in_degrees = True + if reason.startswith("cross_view_roll_curve_difference_too_large:"): + field_name = reason.split(":", 2)[1] + metric_key = f"{field_name}_rms_difference_rad" + metric_limit = float(maximum_rms_difference_rad) + elif reason.startswith( + "cross_view_roll_branch_gap_difference_too_large:" + ): + metric_key = "baseline_branch_gap_difference_rad" + metric_limit = float(maximum_branch_gap_difference_rad) + elif reason.startswith( + "cross_view_roll_projection_scale_ratio_too_large:" + ): + metric_key = "projection_scale_ratio" + metric_limit = float(maximum_projection_scale_ratio) + metric_in_degrees = False + + cycle_values_rad: list[float] = [] + cycle_direction_disagrees: list[bool] = [] + for primary_cycle, validation_cycle in cycle_fits: + try: + cycle_metrics = compare_cross_view_roll_curves( + primary_cycle, + validation_cycle, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=float("inf"), + ) + except ValueError as cycle_error: + if str(cycle_error) != "cross_view_roll_curve_direction_disagrees": + raise + cycle_metrics = {} + if metric_key is not None: + value = cycle_metrics.get(metric_key) + if value is not None: + cycle_values_rad.append(float(value)) + primary_travel = float( + primary_cycle.angle_rad[0] - primary_cycle.angle_rad[255] + ) + validation_travel = float( + validation_cycle.angle_rad[0] + - validation_cycle.angle_rad[255] + ) + cycle_direction_disagrees.append( + primary_travel * validation_travel <= 0.0 + ) + + if metric_key is not None and metric_limit is not None: + actual_value = float(metrics[metric_key]) + rendered_actual = ( + math.degrees(actual_value) + if metric_in_degrees + else actual_value + ) + rendered_limit = ( + math.degrees(metric_limit) + if metric_in_degrees + else metric_limit + ) + failure.update( + { + "actual": round(rendered_actual, 6), + "limit": round(rendered_limit, 6), + "comparison": "maximum", + "unit": "deg" if metric_in_degrees else "ratio", + } + ) + if cycle_values_rad: + values = np.asarray(cycle_values_rad, dtype=float) + cycle_key = ( + "cycle_values_deg" + if metric_in_degrees + else "cycle_values_ratio" + ) + failure[cycle_key] = [ + round( + math.degrees(value) if metric_in_degrees else value, + 6, + ) + for value in values + ] + over = np.flatnonzero(values > metric_limit) + if ( + over.size == 1 + and np.all( + np.delete(values, over[0]) <= 0.9 * metric_limit + ) + ): + failure["cycle"] = int(over[0]) + 1 + # All cycles tell the same near-threshold story: this is a fixed + # view/mount/model bias, not random data that another scan heals. + if ( + np.all(values >= 0.85 * metric_limit) + and float(np.ptp(values)) <= 0.35 * metric_limit + ): + failure["systematic"] = True + failure["classification"] = ( + "stable_cross_view_installation_or_model_bias" + ) + elif reason == "cross_view_roll_curve_direction_disagrees" and ( + cycle_direction_disagrees + and all(cycle_direction_disagrees) + ): + failure["systematic"] = True + failure["classification"] = "stable_cross_view_direction_conflict" + return failure + + def _fit_failure_repeats_branch_clusters( previous: Sequence[Mapping[str, Any]], current: Sequence[Mapping[str, Any]], @@ -1095,6 +1526,58 @@ class SweepItem: return 0 if self.direction == DIRECTION_DECREASING else 255 +def _build_sweep_plan( + profile: HandCalibrationProfile, repetitions: int +) -> tuple[SweepItem, ...]: + """Build one deterministic qualification-and-measurement transaction. + + Each product task owns a low-speed outbound/return qualification followed + by fixed-order formal cycles. Keeping this construction in one place + prevents resume, progress and runtime transitions from silently inventing + different task lifecycles. + """ + plan: list[SweepItem] = [] + for spec in profile.sweep_specs: + if profile.supports("precheck_sweeps"): + plan.extend( + SweepItem(spec, -1, direction, precheck=True) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + plan.extend( + SweepItem(spec, cycle, direction) + for cycle in range(int(repetitions)) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + return tuple(plan) + + +def _sweep_plan_transition( + profile: HandCalibrationProfile, + completed: SweepItem, + following: SweepItem, +) -> str: + """Return the only allowed boundary between two planned sweep items.""" + if completed.spec != following.spec: + return "task_change" + # Both qualification directions and the qualification-to-formal handoff + # are one continuous transaction. The physical endpoint and task + # clearance pose are unchanged, so a baseline detour can only discard + # good observations and perturb the PnP branch. + if completed.precheck: + return "immediate_reverse" + if completed.direction == DIRECTION_INCREASING: + # Formal cycles deliberately re-establish the same mechanical approach + # between repetitions so hysteresis measurements remain comparable. + return "cycle_reset" + return "immediate_reverse" + + @dataclass(frozen=True) class ValidationItem: spec: SweepSpec @@ -1252,6 +1735,15 @@ class ViewRuntime: latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) latest_pnp_rejections: dict[str, str] = field(default_factory=dict) latest_group_pnp_reason: str = "" + latest_group_missing_candidate_roles: tuple[str, ...] = () + # Task-scoped counters preserve the reasons that occurred before the last + # camera frame. A final ``group_initializing:5/8`` frame must not hide a + # recurring per-Tag rejection or a completed-window geometry rejection. + pnp_rejection_counts: dict[str, int] = field(default_factory=dict) + group_pnp_rejection_counts: dict[str, int] = field(default_factory=dict) + pnp_initialization_progress: tuple[int, int] | None = None + latest_pnp_valid: bool = False + last_pnp_diagnostic_signature: tuple[Any, ...] | None = None pnp_invalid_since: float | None = None pnp_reset_count: int = 0 last_message_at: float = 0.0 @@ -1349,6 +1841,9 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_items: list[SweepItem] = [] self.sweep_index = 0 self.active_sweep: SweepItem | None = None + # Owns required Tag roles and PnP continuity across the brief + # active_sweep=None interval used for same-task cycle resets. + self.pnp_task_spec: SweepSpec | None = None self.sweep_frames: list[FrameObservation] = [] # Roll joints use command 127 as their zero. A frame acquired while # merely passing 127 contains velocity/latency error and is not a @@ -1368,6 +1863,11 @@ class G20ThreeCameraCalibrationNode(Node): # target is commanded immediately, a fast motor can leave the endpoint # before the next camera/state pair arrives. self.sweep_start_frames: list[FrameObservation] = [] + # A completed direction already supplies fresh, synchronised frames at + # the physical endpoint where the immediately reversed direction + # starts. Stage those frames across the state transition instead of + # discarding them and waiting again at the unchanged pose. + self.carried_sweep_start_frames: list[FrameObservation] = [] self.sweep_started_at = 0.0 self.sweep_last_valid_at = 0.0 self.sweep_last_valid_at_by_view: dict[str, float] = {} @@ -1408,10 +1908,21 @@ class G20ThreeCameraCalibrationNode(Node): self.command_records_by_joint: dict[str, list[dict[str, Any]]] = { name: [] for name in self.profile.record_joints } + self.palm_axis_records_by_source: dict[ + str, list[dict[str, Any]] + ] = { + observer.source_name: [] + for observer in self.profile.palm_axis_observers + } self.measured_fits: dict[str, JointCurveFit] = {} self.axis_measurements: list[JointAxisMeasurement] = [] + self.palm_orientation_measurements: list[ + PalmOrientationMeasurement + ] = [] + self.palm_orientation_rejections: dict[str, str] = {} self.zero_result: ZeroSolveResult | None = None + self.validated_endpoint_zero_offsets_rad: dict[str, float] = {} self.corrected_urdf_path: Path | None = None self.fit_quality_passed = False @@ -1482,6 +1993,7 @@ class G20ThreeCameraCalibrationNode(Node): ) def _declare_parameters(self) -> None: + self.declare_parameter("model", "G20") self.declare_parameter("hand_type", "left") self.declare_parameter("tag_layout", "legacy_11") self.declare_parameter("serial_number", "UNSET") @@ -1570,8 +2082,11 @@ class G20ThreeCameraCalibrationNode(Node): ) self.declare_parameter("zero_maximum_offset_deg", 20.0) self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) - self.declare_parameter("flexion_endpoint_maximum_offset_deg", 5.0) + self.declare_parameter("mechanical_endpoint_maximum_offset_deg", 5.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter( + "synchronised_endpoint_tolerance_margin_u8", 1.0 + ) self.declare_parameter( "steady_checkpoint_command_feedback_tolerance_u8", 8.0 ) @@ -1632,20 +2147,15 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter( "cross_view_roll_maximum_branch_gap_difference_deg", 0.3 ) + self.declare_parameter( + "cross_view_roll_maximum_shape_rms_deg", 1.25 + ) + self.declare_parameter( + "cross_view_roll_maximum_projection_scale_ratio", 1.5 + ) self.declare_parameter( "cross_view_roll_maximum_axis_difference_deg", 15.0 ) - self.declare_parameter( - # The front roll-link axis line and the side PIP-link axis line - # sit ~21 mm apart systematically (session 20260820_132727: - # 20.75-21.13 mm over eight attempts) because the splay carriage - # carries a screw translation; keep the gross bound above that - # systematic while still failing on gross misalignment. - "cross_view_roll_maximum_axis_line_difference_mm", 30.0 - ) - self.declare_parameter( - "cross_view_roll_alias_maximum_branch_gap_range_deg", 0.5 - ) self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) self.declare_parameter("passive_maximum_hysteresis_deg", 7.5) self.declare_parameter("command_maximum_direction_gap_deg", 2.0) @@ -1678,14 +2188,14 @@ class G20ThreeCameraCalibrationNode(Node): return () return () if result is None else result + self.model = str(value("model")).strip().upper() self.hand_type = str(value("hand_type")).lower() self.tag_layout = str(value("tag_layout")).lower() - self.profile: HandCalibrationProfile = get_hand_calibration_profile( - self.hand_type, self.tag_layout - ) - self.zero_profile: ZeroCalibrationProfile = ( - get_zero_calibration_profile(self.hand_type, self.tag_layout) + product_contract = get_product_calibration_contract( + self.model, self.hand_type, self.tag_layout ) + self.profile = product_contract.profile + self.zero_profile = product_contract.zero_profile self.serial_number = str(value("serial_number")) if self.serial_number == "UNSET": raise ValueError("serial_number is required") @@ -1896,11 +2406,14 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_finger_maximum_offset_rad = math.radians( float(value("zero_finger_maximum_offset_deg")) ) - self.flexion_endpoint_maximum_offset_rad = math.radians( - float(value("flexion_endpoint_maximum_offset_deg")) + self.mechanical_endpoint_maximum_offset_rad = math.radians( + float(value("mechanical_endpoint_maximum_offset_deg")) ) self.zero_joint_maximum_offsets_rad = {} self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.synchronised_endpoint_tolerance_margin_u8 = float( + value("synchronised_endpoint_tolerance_margin_u8") + ) self.steady_checkpoint_command_feedback_tolerance_u8 = float( value("steady_checkpoint_command_feedback_tolerance_u8") ) @@ -2010,15 +2523,15 @@ class G20ThreeCameraCalibrationNode(Node): self.cross_view_roll_maximum_branch_gap_difference_rad = math.radians( float(value("cross_view_roll_maximum_branch_gap_difference_deg")) ) + self.cross_view_roll_maximum_shape_rms_rad = math.radians( + float(value("cross_view_roll_maximum_shape_rms_deg")) + ) + self.cross_view_roll_maximum_projection_scale_ratio = float( + value("cross_view_roll_maximum_projection_scale_ratio") + ) self.cross_view_roll_maximum_axis_difference_rad = math.radians( float(value("cross_view_roll_maximum_axis_difference_deg")) ) - self.cross_view_roll_maximum_axis_line_difference_m = 0.001 * float( - value("cross_view_roll_maximum_axis_line_difference_mm") - ) - self.cross_view_roll_alias_maximum_branch_gap_range_rad = math.radians( - float(value("cross_view_roll_alias_maximum_branch_gap_range_deg")) - ) self.passive_maximum_monotonic_correction_rad = math.radians( float(value("passive_maximum_monotonic_correction_deg")) ) @@ -2059,13 +2572,17 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_confidence_half_width_rad = math.radians( float(value("zero_maximum_confidence_half_width_deg")) ) - if len(self.baseline_command) != 20: - raise ValueError("baseline_command_u8 must contain exactly 20 values") + if len(self.baseline_command) != self.profile.command_count: + raise ValueError( + "baseline_command_u8 length does not match the registered " + f"product command schema ({self.profile.command_count})" + ) if any(value < 0 or value > 255 for value in self.baseline_command): raise ValueError("baseline_command_u8 values must be in [0, 255]") - if self.baseline_command != THREE_CAMERA_BASELINE_COMMAND: + if self.baseline_command != tuple(self.profile.baseline_command): raise ValueError( - "schema v4 requires the fixed G20 three-camera baseline" + "baseline_command_u8 differs from the registered product " + "calibration baseline" ) if any(not serial for serial in self.camera_serials.values()): raise ValueError("front/side/top camera serial parameters are required") @@ -2145,6 +2662,10 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("speed_setting_settle_seconds must be non-negative") if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") + if not 0.0 <= self.synchronised_endpoint_tolerance_margin_u8 <= 4.0: + raise ValueError( + "synchronised_endpoint_tolerance_margin_u8 must be in [0, 4]" + ) if not ( self.endpoint_tolerance_u8 <= self.steady_checkpoint_command_feedback_tolerance_u8 @@ -2217,7 +2738,7 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cone_mismatch_rad, self.zero_maximum_offset_rad, self.zero_finger_maximum_offset_rad, - self.flexion_endpoint_maximum_offset_rad, + self.mechanical_endpoint_maximum_offset_rad, self.maximum_validation_error_rad, self.zero_maximum_confidence_half_width_rad, self.task_precheck_hold_seconds, @@ -2229,12 +2750,17 @@ class G20ThreeCameraCalibrationNode(Node): self.directional_zero_maximum_branch_gap_rad, self.directional_zero_maximum_branch_gap_range_rad, self.cross_view_roll_maximum_branch_gap_difference_rad, + self.cross_view_roll_maximum_shape_rms_rad, self.passive_maximum_monotonic_correction_rad, self.passive_maximum_hysteresis_rad, self.command_maximum_direction_gap_rad, ) ): raise ValueError("trajectory quality thresholds must be positive") + if not 1.0 < self.cross_view_roll_maximum_projection_scale_ratio <= 2.0: + raise ValueError( + "cross-view roll projection-scale ratio must be in (1, 2]" + ) if self.zero_finger_maximum_offset_rad > self.zero_maximum_offset_rad: raise ValueError( "zero_finger_maximum_offset_deg cannot exceed " @@ -2327,26 +2853,15 @@ class G20ThreeCameraCalibrationNode(Node): ) if pair[0] in role_set and pair[1] in role_set ) - # The side-view palm/reference-finger Tags are mounted on parallel - # outward-facing surfaces at the endpoint where a task tracker is - # initialized. Temporal rigidity alone cannot distinguish two fully - # static IPPE mirror branches; their face-normal consistency can. Do - # not apply this prior to the articulated front thumb chain. - normal_alignment_pairs = ( - tuple( - pair - for pair in adjacent_pairs - # Distal 16 mm Tags may be carried by a rigid extension rod - # to remain visible through PIP flexion. Its mounting plane - # is arbitrary and must not be required to be parallel to the - # middle-phalanx Tag. Static rigidity and relative-pose - # continuity still disambiguate that PIP->DIP pair; retain - # the palm/base->middle-Tag normal prior unchanged. - if not pair[1].endswith("_dip") - ) - if name == "side" and len(role_tuple) <= 4 - else () - ) + # Do not impose an absolute face-normal relationship between the + # fixed palm Tag and an articulated finger Tag. That angle depends on + # the physical Tag bracket and the endpoint joint pose; it is not a + # product invariant. The old "nearly parallel" prior repeatedly + # cleared otherwise stable 8-frame initialisation windows for ID4/ID5 + # on ring MCP flexion. Multi-frame relative rigidity, reprojection + # error and the task reference retained across cycles provide the + # valid branch evidence without assuming a particular mounting plane. + normal_alignment_pairs: tuple[tuple[str, str], ...] = () thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"} thumb_mcp_ip_coupling = () if name == "front" and thumb_mcp_ip_roles.issubset(role_set): @@ -2456,6 +2971,14 @@ class G20ThreeCameraCalibrationNode(Node): and view in _sweep_views(_node_profile(self), self.retry_sweep_spec) ): active_spec = self.retry_sweep_spec + elif ( + getattr(self, "pnp_task_spec", None) is not None + and view + in _sweep_views( + _node_profile(self), self.pnp_task_spec + ) + ): + active_spec = self.pnp_task_spec if active_spec is None: return tuple(runtime.preflight_roles) required: set[str] = set() @@ -2495,6 +3018,8 @@ class G20ThreeCameraCalibrationNode(Node): spec = self.active_validation.spec elif self.retry_sweep_spec is not None: spec = self.retry_sweep_spec + elif getattr(self, "pnp_task_spec", None) is not None: + spec = self.pnp_task_spec if spec is None or not _sweep_uses_locked_base_reference( _node_profile(self), spec, view ): @@ -2618,6 +3143,162 @@ class G20ThreeCameraCalibrationNode(Node): preserve_task_reference=preserve_task_reference ) + @staticmethod + def _reset_view_pnp_diagnostics(runtime: ViewRuntime) -> None: + """Clear PnP evidence only when a new capture scope begins.""" + runtime.latest_pnp_rejections.clear() + runtime.latest_group_pnp_reason = "" + runtime.latest_group_missing_candidate_roles = () + runtime.pnp_rejection_counts.clear() + runtime.group_pnp_rejection_counts.clear() + runtime.pnp_initialization_progress = None + runtime.latest_pnp_valid = False + runtime.last_pnp_diagnostic_signature = None + + def _record_group_pnp_candidate_event( + self, + *, + runtime: ViewRuntime, + stamp_ns: int, + required_roles: Sequence[str], + pose_roles: Sequence[str], + corners_by_role: Mapping[str, np.ndarray], + qualities: Mapping[str, TagQuality], + matched_tracking: tuple[tuple[float, ...], int] | None, + selected: Mapping[str, SquareTagPose] | None, + pnp_rejections: Mapping[str, str], + group_pnp_reason: str, + ) -> None: + """Persist bounded, replayable evidence for group candidate decisions. + + Valid FrameObservation records intentionally contain only accepted + poses. Without a separate event, a deterministic PnP dropout leaves + no corners, per-candidate tilt, reprojection error or missing role in + raw_samples.jsonl, making the next run guess at the cause. Record one + event per eight-count motor bucket and decision signature. This is + dense enough to locate a repeatable geometry boundary without writing + every 30 Hz rejected camera message. + """ + item = self.active_sweep + if ( + item is None + or self.state not in {STATE_PREPARE_SWEEP, STATE_SWEEP} + or runtime.name not in _sweep_views(_node_profile(self), item.spec) + ): + return + candidate_diagnostics = { + role: dict( + runtime.tracker.last_candidate_diagnostics_by_role.get( + role, {} + ) + ) + for role in pose_roles + if runtime.tracker.last_candidate_diagnostics_by_role.get(role) + } + oblique_group_roles = tuple( + role + for role, diagnostics in candidate_diagnostics.items() + if int(diagnostics.get("reprojection_candidate_count", 0)) > 0 + and int( + diagnostics.get("independent_tilt_candidate_count", 0) + ) + == 0 + ) + if ( + not group_pnp_reason + and not pnp_rejections + and not oblique_group_roles + ): + return + if selected is not None and not oblique_group_roles: + return + state_u8 = ( + matched_tracking[0] + if matched_tracking is not None + else self.latest_state_u8 + ) + motor_value = ( + None + if len(state_u8) != 20 + else float(state_u8[item.spec.motor_index]) + ) + motor_bucket = ( + -1 + if motor_value is None or not math.isfinite(motor_value) + else int(np.clip(math.floor(motor_value / 8.0), 0, 31)) + ) + missing_roles = tuple(runtime.latest_group_missing_candidate_roles) + signature = ( + item.spec.key, + item.cycle, + item.direction, + motor_bucket, + selected is not None, + str(group_pnp_reason), + tuple( + sorted( + (str(key), str(value)) + for key, value in pnp_rejections.items() + ) + ), + missing_roles, + oblique_group_roles, + ) + if runtime.last_pnp_diagnostic_signature == signature: + return + runtime.last_pnp_diagnostic_signature = signature + append_jsonl( + self.raw_path, + { + "kind": "group_pnp_candidate_event", + "outcome": ( + "selected_with_oblique_group_constraint" + if selected is not None + else "group_pose_rejected" + ), + "view": runtime.name, + "task_name": item.spec.key, + "motor_index": item.spec.motor_index, + "cycle": item.cycle, + "direction": item.direction, + "feedback_u8": motor_value, + "feedback_bucket_u8": ( + None if motor_bucket < 0 else motor_bucket * 8 + ), + "image_stamp_ns": int(stamp_ns), + "required_roles": list(required_roles), + "required_tag_ids": { + role: runtime.view_tags[role] + for role in required_roles + }, + "group_pnp_reason": str(group_pnp_reason), + "group_missing_candidate_roles": list(missing_roles), + "pnp_rejections": dict(pnp_rejections), + "oblique_group_roles": list(oblique_group_roles), + "candidate_diagnostics": candidate_diagnostics, + "tag_quality": { + role: { + "hamming": int(quality.hamming), + "decision_margin": float(quality.decision_margin), + "mean_edge_pixels": float(quality.edge_pixels), + } + for role, quality in qualities.items() + if role in required_roles + }, + "corners_xy": { + role: np.asarray(corners, dtype=float).tolist() + for role, corners in corners_by_role.items() + if role in required_roles + }, + "camera_intrinsics_sha256": runtime.intrinsics_sha256, + "camera_matrix": ( + None + if runtime.camera_matrix is None + else runtime.camera_matrix.tolist() + ), + }, + ) + def _camera_info_callback(self, view: str, message: CameraInfo) -> None: runtime = self.views[view] valid = ( @@ -2669,11 +3350,15 @@ class G20ThreeCameraCalibrationNode(Node): ) def _state_callback(self, message: JointState) -> None: - if len(message.position) != 20: + command_names = _command_names(self) + if len(message.position) != len(command_names): return - if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): + if ( + len(message.name) == len(command_names) + and set(message.name) == set(command_names) + ): lookup = dict(zip(message.name, message.position)) - state = tuple(float(lookup[name]) for name in COMMAND_NAMES) + state = tuple(float(lookup[name]) for name in command_names) else: state = tuple(float(value) for value in message.position) stamp = _stamp_ns(message.header.stamp) @@ -2715,6 +3400,19 @@ class G20ThreeCameraCalibrationNode(Node): live_required_roles = self._live_required_roles_for_view( view, required_roles ) + palm_observer: PalmAxisObserver | None = None + if ( + self.active_sweep is not None + and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + ): + palm_observer = _palm_axis_observer_for_sweep( + _node_profile(self), self.active_sweep.spec, view + ) + palm_observer_roles = ( + () + if palm_observer is None + else (palm_observer.parent_role, palm_observer.child_role) + ) combination_capture = bool( self.active_combination_validation is not None and self.state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE} @@ -2723,8 +3421,7 @@ class G20ThreeCameraCalibrationNode(Node): runtime.current_required_roles = required_roles runtime.valid_flags.clear() runtime.detection_times.clear() - runtime.latest_pnp_rejections.clear() - runtime.latest_group_pnp_reason = "" + self._reset_view_pnp_diagnostics(runtime) runtime.task_valid_frames = 0 runtime.task_total_frames = 0 self._reset_view_trackers(runtime) @@ -2759,7 +3456,22 @@ class G20ThreeCameraCalibrationNode(Node): and self._quality_valid(qualities[role], include_pnp=False) ) if combination_capture - else live_required_roles + else tuple( + dict.fromkeys( + ( + *live_required_roles, + *( + role + for role in palm_observer_roles + if role in qualities + and role != locked_base_role + and self._quality_valid( + qualities[role], include_pnp=False + ) + ), + ) + ) + ) ) observable_combination_joints = ( _combination_observable_joints( @@ -2789,7 +3501,12 @@ class G20ThreeCameraCalibrationNode(Node): matched_tracking: tuple[tuple[float, ...], int] | None = None if ( self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + and ( + view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + or palm_observer is not None + ) and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} ): matched_tracking = interpolate_state_u8( @@ -2842,13 +3559,19 @@ class G20ThreeCameraCalibrationNode(Node): camera_matrix=runtime.camera_matrix, stamp_ns=stamp, ) - if rejection: - pnp_rejections[role] = rejection if pose is not None: individual_selected[role] = pose candidates[role] = runtime.tracker.last_candidates_by_role.get( role, () ) + # A group-constrained task deliberately retains an oblique + # candidate that the independent per-Tag selector would not + # use. Report a rejection only if candidate generation itself + # failed; otherwise the group tracker owns the decision. + if rejection and ( + combination_capture or not candidates[role] + ): + pnp_rejections[role] = rejection if combination_capture: # Each visible target is tracked independently. This avoids # an exponential all-Tag IPPE search and, more importantly, @@ -2869,6 +3592,26 @@ class G20ThreeCameraCalibrationNode(Node): trajectory_command_u8=tracking_command_u8, trajectory_direction=tracking_direction, ) + runtime.latest_group_missing_candidate_roles = ( + group_tracker.last_missing_roles + ) + if selected is not None and palm_observer_roles: + selected = dict(selected) + for role in palm_observer_roles: + pose = individual_selected.get(role) + quality = qualities.get(role) + if pose is None or quality is None: + continue + pose_quality = TagQuality( + hamming=quality.hamming, + decision_margin=quality.decision_margin, + edge_pixels=quality.edge_pixels, + reprojection_error_px=pose.reprojection_error_px, + ) + if self._quality_valid( + pose_quality, include_pnp=True + ): + selected[role] = pose if selected is not None: qualities = _selected_pose_qualities( selected, @@ -2899,7 +3642,11 @@ class G20ThreeCameraCalibrationNode(Node): and all(role in selected for role in required_roles) and all( self._quality_valid(qualities[role], include_pnp=True) - for role in selected + for role in ( + selected + if combination_capture + else required_roles + ) ) and ( not combination_capture @@ -2934,6 +3681,54 @@ class G20ThreeCameraCalibrationNode(Node): valid_by_view[view] = valid_by_view.get(view, 0) + 1 runtime.latest_pnp_rejections = pnp_rejections runtime.latest_group_pnp_reason = group_pnp_reason + if not group_pnp_reason: + runtime.latest_group_missing_candidate_roles = () + runtime.latest_pnp_valid = valid + task_pnp_capture = bool( + self.active_sweep is not None + and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + if task_pnp_capture: + for role, rejection in pnp_rejections.items(): + key = f"{role}:{rejection}" + runtime.pnp_rejection_counts[key] = ( + runtime.pnp_rejection_counts.get(key, 0) + 1 + ) + if group_pnp_reason.startswith("group_initializing:"): + try: + progress = group_pnp_reason.split(":", 1)[1] + accepted, required = progress.split("/", 1) + runtime.pnp_initialization_progress = ( + int(accepted), + int(required), + ) + except (IndexError, TypeError, ValueError): + runtime.pnp_initialization_progress = None + elif group_pnp_reason: + runtime.group_pnp_rejection_counts[group_pnp_reason] = ( + runtime.group_pnp_rejection_counts.get( + group_pnp_reason, 0 + ) + + 1 + ) + elif selected is not None and runtime.pnp_initialization_progress: + required = runtime.pnp_initialization_progress[1] + runtime.pnp_initialization_progress = (required, required) + self._record_group_pnp_candidate_event( + runtime=runtime, + stamp_ns=stamp, + required_roles=required_roles, + pose_roles=pose_roles, + corners_by_role=corners_by_role, + qualities=qualities, + matched_tracking=matched_tracking, + selected=selected, + pnp_rejections=pnp_rejections, + group_pnp_reason=group_pnp_reason, + ) if view == "top": runtime.pnp_invalid_since, reset_due = update_pnp_reset_watchdog( detection_good=bool( @@ -3013,7 +3808,16 @@ class G20ThreeCameraCalibrationNode(Node): sweep_capture = bool( self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} and self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + palm_axis_capture = bool( + self.state == STATE_SWEEP + and self.active_sweep is not None + and not self.active_sweep.precheck + and self.active_sweep.cycle >= 0 + and palm_observer is not None ) validation_capture = bool( self.state == STATE_VALIDATION_CAPTURE @@ -3027,7 +3831,7 @@ class G20ThreeCameraCalibrationNode(Node): or self.active_combination_validation is not None ) ) - if not sweep_capture and not validation_capture: + if not sweep_capture and not validation_capture and not palm_axis_capture: return matched = matched_tracking @@ -3049,6 +3853,17 @@ class G20ThreeCameraCalibrationNode(Node): if self.extrinsics is None or not runtime.extrinsics_valid: return front_from_view = self.extrinsics.transform(view) + if palm_axis_capture and palm_observer is not None: + self._record_palm_axis_sample( + palm_observer, + selected, + front_from_view, + state_u8, + sync_error_ns=int(sync_error_ns), + stamp_ns=stamp, + ) + if not sweep_capture and not validation_capture: + return for name, spec in self.profile.record_specs.items(): if not spec.measured or spec.view != view: continue @@ -3104,6 +3919,113 @@ class G20ThreeCameraCalibrationNode(Node): ) self._accept_frame(observation) + def _record_palm_axis_sample( + self, + observer: PalmAxisObserver, + selected: Mapping[str, SquareTagPose], + common_from_view: np.ndarray, + state_u8: Sequence[float], + *, + sync_error_ns: int, + stamp_ns: int, + ) -> None: + """Buffer one direction-only sample without touching sweep validity.""" + item = self.active_sweep + if ( + item is None + or item.spec.key != observer.task_name + or observer.parent_role not in selected + or observer.child_role not in selected + ): + return + parent = selected[observer.parent_role] + child = selected[observer.child_role] + parent_matrix = common_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = common_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative_rotation = ( + Rotation.from_matrix(parent_matrix[:3, :3]).inv() + * Rotation.from_matrix(child_matrix[:3, :3]) + ) + feedback = float(state_u8[observer.motor_index]) + record = { + "kind": "palm_axis_sample", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": observer.view, + "source_joint": observer.source_name, + "model_joint": observer.model_joint, + "motor_index": int(observer.motor_index), + "cycle": int(item.cycle), + "direction": item.direction, + "command_u8": int(np.clip(np.rint(feedback), 0, 255)), + "relative_quaternion_xyzw": [ + float(value) for value in relative_rotation.as_quat() + ], + "parent_pose_common": matrix_payload(parent_matrix), + "child_pose_common": matrix_payload(child_matrix), + "state_u8": [float(value) for value in state_u8], + "state_image_sync_error_ms": round( + abs(int(sync_error_ns)) / 1_000_000.0, 6 + ), + "pnp_reprojection_error_px": round( + max( + float(parent.reprojection_error_px), + float(child.reprojection_error_px), + ), + 6, + ), + "image_stamp_ns": int(stamp_ns), + } + self.palm_axis_records_by_source.setdefault( + observer.source_name, [] + ).append(record) + + def _persist_palm_axis_samples(self, item: SweepItem) -> None: + """Commit the side channel only after its authoritative sweep passes.""" + observer = next( + ( + candidate + for candidate in self.profile.palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + if observer is None: + return + attempt = int( + self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) + ) + rows = [ + record + for record in self.palm_axis_records_by_source.get( + observer.source_name, () + ) + if int(record.get("attempt", 1)) == attempt + and int(record.get("cycle", -1)) == int(item.cycle) + and str(record.get("direction", "")) == item.direction + ] + append_jsonl_many( + self.raw_path, + ( + { + **{ + key: value + for key, value in record.items() + if key != "command_u8" + }, + "requested_command_u8": int(item.target_u8), + "feedback_u8": int(record["command_u8"]), + } + for record in rows + ), + ) + def _accept_frame(self, observation: FrameObservation) -> None: if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: if observation.view not in _sweep_views( @@ -3119,7 +4041,11 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_start_frames.append(observation) maximum_start_frames = max( 1, int(getattr(self, "preflight_frames", 30)) - ) * len(_sweep_views(_node_profile(self), self.active_sweep.spec)) + ) * len( + _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) if len(self.sweep_start_frames) > maximum_start_frames: del self.sweep_start_frames[:-maximum_start_frames] return @@ -3296,7 +4222,7 @@ class G20ThreeCameraCalibrationNode(Node): def _resume_preflight_ready(self, now: float) -> bool: if ( - len(self.latest_state_u8) != 20 + len(self.latest_state_u8) != _command_count(self) or now - self.last_state_at > 1.0 ): return False @@ -3341,6 +4267,23 @@ class G20ThreeCameraCalibrationNode(Node): for record in command_records[joint_name] if int(record.get("cycle", -1)) not in cycles ] + observer = next( + ( + item + for item in _node_profile(self).palm_axis_observers + if item.task_name == spec.key + ), + None, + ) + if observer is not None: + records = self.palm_axis_records_by_source.get( + observer.source_name, [] + ) + records[:] = [ + record + for record in records + if int(record.get("cycle", -1)) not in cycles + ] all_items = getattr(self, "sweep_items", []) self.retry_sweep_items = [ item @@ -3367,13 +4310,18 @@ class G20ThreeCameraCalibrationNode(Node): _node_profile(self).record_specs[name].view for name in retry_joint_names } + if observer is not None: + retry_views.add(observer.view) for view in retry_views: if view is None: continue runtime = getattr(self, "views", {}).get(view) if runtime is None or reset_trackers is None: continue - reset_trackers(runtime) + if _node_profile(self).layout_id == G20_RIGHT_19_LAYOUT: + reset_trackers(runtime, preserve_task_reference=True) + else: + reset_trackers(runtime) runtime.pnp_invalid_since = None runtime.pnp_reset_count += 1 # Detection quality is attempt-local. Carrying the previous @@ -3403,9 +4351,77 @@ class G20ThreeCameraCalibrationNode(Node): }, ) self.fit_failure = {} + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) getattr(self, "motion_stall_details", {}).clear() return spec + def _invalidate_fitted_calibration_state(self) -> None: + """Invalidate every artifact derived from the current motion records. + + A task rescan changes the observation set, so curves, zero offsets and + validation diagnostics must be treated as one generation. Keeping + this transition in one place prevents final publication from mixing a + newly collected task with state left by an earlier fit attempt. + """ + self.measured_fits = {} + self.axis_measurements = [] + self.palm_orientation_measurements = [] + self.palm_orientation_rejections = {} + self.zero_result = None + self.validated_endpoint_zero_offsets_rad = {} + self.corrected_urdf_path = None + self.fit_quality_passed = False + self.validation_errors_rad = [] + self.cross_view_roll_metrics = {} + self.validation_only_fits = {} + self.joint_dynamic_diagnostics = {} + + def _endpoint_zero_offsets_for_publication(self) -> dict[str, float]: + """Return the endpoint anchors belonging to the validated zero fit.""" + stored = { + str(name): float(value) + for name, value in getattr( + self, "validated_endpoint_zero_offsets_rad", {} + ).items() + } + if self.profile.layout_id != G20_RIGHT_19_LAYOUT: + if stored: + raise RuntimeError( + "validated_endpoint_zero_state_unexpected_for_layout" + ) + return {} + expected = set(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) + actual = set(stored) + if actual != expected: + missing = ",".join(sorted(expected - actual)) or "-" + extra = ",".join(sorted(actual - expected)) or "-" + raise RuntimeError( + "validated_endpoint_zero_state_incomplete:" + f"missing={missing};extra={extra}" + ) + if not all(math.isfinite(value) for value in stored.values()): + raise RuntimeError("validated_endpoint_zero_state_non_finite") + if self.zero_result is None: + raise RuntimeError("URDF zero solution is missing") + solved = self.zero_result.direct_offsets_rad + diverged = sorted( + name + for name, value in stored.items() + if name not in solved + or not math.isclose( + value, + float(solved[name]), + rel_tol=0.0, + abs_tol=1.0e-12, + ) + ) + if diverged: + raise RuntimeError( + "validated_endpoint_zero_state_diverged:" + + ",".join(diverged) + ) + return stored + def _restore_durable_task_checkpoint(self) -> int: """Restore independently validated complete tasks from a failed session.""" source = self.resume_raw_samples_path @@ -3439,8 +4455,19 @@ class G20ThreeCameraCalibrationNode(Node): ).items() } current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {})) + try: + ( + palm_axis_schema_compatible, + palm_axis_invalidated_tasks, + ) = _palm_axis_resume_policy(self.profile, start) + except ValueError as error: + raise RuntimeError( + "resume checkpoint algorithm capabilities differ" + ) from error if ( - str(start.get("hand_type")) != self.hand_type + str(start.get("model", "G20")).upper() + != str(getattr(self, "model", "G20")).upper() + or str(start.get("hand_type")) != self.hand_type or str(start.get("tag_layout")) != self.profile.layout_id or start.get("view_tags") != {view: dict(tags) for view, tags in self.profile.view_tags.items()} @@ -3450,7 +4477,8 @@ class G20ThreeCameraCalibrationNode(Node): != _file_sha256(self.source_urdf_path) ): raise RuntimeError( - "resume checkpoint geometry, Tag layout, baseline or source URDF differs" + "resume checkpoint geometry, algorithm capabilities, Tag " + "layout, baseline or source URDF differs" ) try: changed_tag_size_ids, size_invalidated_tasks = ( @@ -3465,6 +4493,7 @@ class G20ThreeCameraCalibrationNode(Node): "resume checkpoint Tag-size geometry differs incompatibly" ) from error invalidated_task_set = set(size_invalidated_tasks) + invalidated_task_set.update(palm_axis_invalidated_tasks) if invalidated_task_set: rows = [ row @@ -3483,6 +4512,19 @@ class G20ThreeCameraCalibrationNode(Node): kind = str(durable["kind"]) if kind == "synchronised_frame": continue + if kind == "palm_axis_sample": + source_name = str(durable.get("source_joint", "")) + if source_name not in self.palm_axis_records_by_source: + raise RuntimeError( + "resume checkpoint contains unknown palm-axis source " + + source_name + ) + record = dict(durable) + record["command_u8"] = int( + round(float(record["feedback_u8"])) + ) + self.palm_axis_records_by_source[source_name].append(record) + continue joint_name = str(durable["joint"]) if joint_name not in self.records_by_joint: raise RuntimeError( @@ -3512,6 +4554,38 @@ class G20ThreeCameraCalibrationNode(Node): for row in reusable if str(row.get("task_name", "")) in completed_set ) + # Attempts are durable identities, not counters local to one process. + # Without this floor a resumed task whose accepted data used attempt 2 + # would label its next retry as attempt 2 again. The raw log then + # contained two different acquisitions with the same identity and an + # offline replay merged them into one invalid trajectory. + attempt_floor_by_task: dict[str, int] = {} + for row in reusable: + task_key = str(row.get("task_name", "")) + if task_key not in completed_set: + continue + try: + attempt = max(1, int(row.get("attempt", 1))) + except (TypeError, ValueError): + attempt = 1 + attempt_floor_by_task[task_key] = max( + attempt_floor_by_task.get(task_key, 1), attempt + ) + attempts = getattr(self, "sweep_attempts", None) + if attempts is None: + self.sweep_attempts = { + _sweep_storage_key(spec): 1 + for spec in self.profile.sweep_specs + } + attempts = self.sweep_attempts + for spec in self.profile.sweep_specs: + if spec.key not in completed_set: + continue + storage_key = _sweep_storage_key(spec) + attempts[storage_key] = max( + int(attempts.get(storage_key, 1)), + attempt_floor_by_task.get(spec.key, 1), + ) self.resumed_task_keys = completed G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) self.resume_source_session = source.parent.name @@ -3534,7 +4608,12 @@ class G20ThreeCameraCalibrationNode(Node): "revalidation_dropped_tasks": dropped, "tag_size_changed_ids": list(changed_tag_size_ids), "tag_size_invalidated_task_keys": list(size_invalidated_tasks), + "palm_axis_schema_compatible": palm_axis_schema_compatible, + "palm_axis_invalidated_task_keys": list( + palm_axis_invalidated_tasks + ), "imported_record_count": len(reusable), + "imported_attempt_floor_by_task": attempt_floor_by_task, "source_raw_samples_sha256": _file_sha256(source), }, ) @@ -3606,6 +4685,18 @@ class G20ThreeCameraCalibrationNode(Node): ): if joint_name in store: store[joint_name].clear() + observer = next( + ( + item + for item in self.profile.palm_axis_observers + if item.task_name == task_key + ), + None, + ) + if observer is not None: + self.palm_axis_records_by_source[ + observer.source_name + ].clear() if isinstance(completed, tuple): accepted = tuple(accepted) return accepted, dropped @@ -3637,6 +4728,7 @@ class G20ThreeCameraCalibrationNode(Node): self.started = True self.startup_baseline_recovered = False self.sweep_items = [] + self.pnp_task_spec = None selected_sweep_specs = list(self.profile.sweep_specs) if self.cross_view_roll_diagnostic_finger: selected_sweep_specs = [ @@ -3649,23 +4741,13 @@ class G20ThreeCameraCalibrationNode(Node): raise RuntimeError( "cross-view diagnostic could not resolve multiview task" ) - for spec in selected_sweep_specs: - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - self.sweep_items.extend( - SweepItem(spec, -1, direction, precheck=True) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - self.sweep_items.extend( - SweepItem(spec, cycle, direction) - for cycle in range(self.repetitions) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) + selected_profile = replace( + self.profile, + sweep_specs=tuple(selected_sweep_specs), + ) + self.sweep_items.extend( + _build_sweep_plan(selected_profile, self.repetitions) + ) self.sweep_index = 0 self.retry_sweep_spec = None self.retry_resume_index = None @@ -3693,10 +4775,11 @@ class G20ThreeCameraCalibrationNode(Node): self.command_records_by_joint = { name: [] for name in self.profile.record_joints } - self.axis_measurements.clear() - self.zero_result = None - self.corrected_urdf_path = None - self.validation_errors_rad.clear() + self.palm_axis_records_by_source = { + observer.source_name: [] + for observer in self.profile.palm_axis_observers + } + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) self.combination_validation_items.clear() self.combination_validation_index = 0 self.active_combination_validation = None @@ -3714,17 +4797,24 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_baseline_pending = False self.sweep_baseline_hold_since = None self.sweep_start_frames.clear() + self.carried_sweep_start_frames.clear() append_jsonl( self.raw_path, { "kind": "session_start", + "model": self.model, "hand_type": self.hand_type, "tag_layout": self.profile.layout_id, + "command_names": list(_command_names(self)), + "capabilities": sorted(self.profile.capabilities), "reference_finger": self.profile.reference_finger, "view_tags": { view: dict(tags) for view, tags in self.profile.view_tags.items() }, + "palm_axis_observers": _palm_axis_observer_schema( + self.profile + ), "tag_family": "36h11", "tag_size_m": float(self.tag_size_m), "tag_sizes_m_by_id": { @@ -3911,7 +5001,7 @@ class G20ThreeCameraCalibrationNode(Node): return message = JointState() message.header.stamp = self.get_clock().now().to_msg() - message.name = list(COMMAND_NAMES) + message.name = list(_command_names(self)) message.position = [float(value) for value in values] self.command_publisher.publish(message) @@ -4071,9 +5161,17 @@ class G20ThreeCameraCalibrationNode(Node): if not required.issubset(by_direction): return - enabled = bool( + profile = _node_profile(self) + configured_enabled = bool( getattr(self, "adaptive_formal_speed_enabled", False) ) + # G20 right repeatability is evaluated across independent sessions. + # Letting camera-dependent precheck density alter the physical scan + # speed changes velocity lag and backlash, so two runs of the same hand + # no longer measure the same process. Keep the reviewed base speed for + # every product task; adaptive speed remains available to legacy tools. + stability_speed_lock = profile.layout_id == G20_RIGHT_19_LAYOUT + enabled = configured_enabled and not stability_speed_lock # MCP roll is both the fastest native mechanism and the one guarded by # the strict 0.5-degree baseline backlash limit. Field data at speed # 7 exceeded that limit after a clean speed-5 precheck, so sampling @@ -4124,10 +5222,14 @@ class G20ThreeCameraCalibrationNode(Node): "task_name": item.spec.key, "motor_index": int(item.spec.motor_index), "enabled": enabled, + "configured_enabled": configured_enabled, + "stability_speed_lock": stability_speed_lock, "eligible": eligible, "ineligible_reason": ( - "roll_baseline_hysteresis_sensitive" - if enabled and not eligible + "g20_right_deterministic_acquisition_speed" + if stability_speed_lock + else "roll_baseline_hysteresis_sensitive" + if configured_enabled and not eligible else "" ), "base_speed": base_speed, @@ -4203,6 +5305,10 @@ class G20ThreeCameraCalibrationNode(Node): def _begin_return_baseline(self, after: str) -> None: self.baseline_after = str(after) + if str(after) not in {"next_cycle", "resume_sweep"}: + # A mechanical task boundary is also the PnP ownership boundary. + # Same-task cycle resets and localized fit recovery retain it. + self.pnp_task_spec = None target_command = ( G20ThreeCameraCalibrationNode._return_command_for_transition( self, after @@ -4881,6 +5987,24 @@ class G20ThreeCameraCalibrationNode(Node): self, spec.motor_index, int(endpoint_u8) ) + def _synchronised_endpoint_tolerance_for_spec( + self, spec: SweepSpec, endpoint_u8: int + ) -> float: + """Return the endpoint bin deadband for image-timestamped feedback. + + Motion completion deliberately keeps the physical endpoint deadband. + A synchronised image observation uses interpolated feedback at the + camera timestamp and can trail that latest feedback by a fraction of + one u8 count. One additional count prevents a reached endpoint from + being discarded solely at this continuous-to-integer bin boundary. + """ + mechanical_tolerance = self._endpoint_tolerance_for_spec( + spec, endpoint_u8 + ) + return mechanical_tolerance + float( + getattr(self, "synchronised_endpoint_tolerance_margin_u8", 1.0) + ) + def _motor_endpoint_tolerance( self, motor_index: int, endpoint_u8: int ) -> float: @@ -4939,6 +6063,11 @@ class G20ThreeCameraCalibrationNode(Node): return baseline not in {item.start_u8, item.target_u8} def _start_next_sweep(self) -> None: + carried_start_frames = list( + getattr(self, "carried_sweep_start_frames", ()) + ) + if hasattr(self, "carried_sweep_start_frames"): + self.carried_sweep_start_frames.clear() retry_items = getattr(self, "retry_sweep_items", []) if retry_items: self.active_sweep = retry_items.pop(0) @@ -4952,13 +6081,37 @@ class G20ThreeCameraCalibrationNode(Node): self.active_sweep = self.sweep_items[self.sweep_index] self.active_sweep_is_fit_retry = False item = self.active_sweep - if item.direction == DIRECTION_DECREASING: + profile = _node_profile(self) + if profile.layout_id == G20_RIGHT_19_LAYOUT: + self.pnp_task_spec = item.spec + is_fit_retry = bool( + getattr(self, "active_sweep_is_fit_retry", False) + ) + if _requires_pnp_tracker_reset_for_sweep( + profile, + item, + is_fit_retry=is_fit_retry, + ): reset_trackers = getattr(self, "_reset_view_trackers", None) - preserve_task_reference = bool( - not item.precheck and item.cycle > 0 + preserve_task_reference = _preserve_pnp_task_reference_for_sweep( + item, + is_fit_retry=is_fit_retry, + has_precheck_anchor=( + profile.layout_id == G20_RIGHT_19_LAYOUT + ), ) - reset_views = set(_sweep_views(_node_profile(self), item.spec)) - if getattr(self, "active_sweep_is_fit_retry", False): + reset_views = set(_sweep_views(profile, item.spec)) + observer = next( + ( + candidate + for candidate in profile.palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + if observer is not None: + reset_views.add(observer.view) + if is_fit_retry: scoped = set(getattr(self, "retry_joint_names", set())) if scoped: reset_views = { @@ -4966,21 +6119,24 @@ class G20ThreeCameraCalibrationNode(Node): for name in scoped } reset_views.discard(None) + if observer is not None: + reset_views.add(observer.view) for view in reset_views: runtime = getattr(self, "views", {}).get(view) if runtime is None or reset_trackers is None: continue - # Clear frame-to-frame state so every cycle receives a full - # static initialization window, but retain the first formal - # cycle's endpoint-relative branch anchor. Without that - # task-level anchor, later cycles can independently settle on - # opposite stable IPPE mirror solutions. + # A normal G20-right task enters here only for its first + # decreasing precheck. All formal cycles keep this exact + # frame-to-frame branch instead of independently selecting a + # new stable IPPE solution. A fit retry deliberately starts + # fresh but retains the task-relative anchor. if preserve_task_reference: reset_trackers( runtime, preserve_task_reference=True ) else: reset_trackers(runtime) + self._reset_view_pnp_diagnostics(runtime) runtime.pnp_invalid_since = None runtime.pnp_reset_count += 1 raw_path = getattr(self, "raw_path", None) @@ -4988,10 +6144,12 @@ class G20ThreeCameraCalibrationNode(Node): append_jsonl( raw_path, { - "kind": "pnp_cycle_initialization", + "kind": "pnp_task_initialization", "view": view, + "task_name": item.spec.key, "motor_index": item.spec.motor_index, "cycle": item.cycle, + "fit_retry": is_fit_retry, "task_reference_preserved": ( preserve_task_reference ), @@ -5002,6 +6160,11 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_baseline_pending = False self.sweep_baseline_hold_since = None getattr(self, "sweep_start_frames", []).clear() + getattr(self, "sweep_start_frames", []).extend(carried_start_frames) + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view = {} + self.sweep_detection_valid_by_view = {} self.position_hold_since = None self.motion_stage_started_at = time.monotonic() current_state = getattr(self, "latest_state_u8", ()) @@ -5044,6 +6207,62 @@ class G20ThreeCameraCalibrationNode(Node): ) self._publish_command(list(self.preparation_command_u8)) + def _stage_immediate_reverse_start_frames( + self, completed: SweepItem, following: SweepItem + ) -> int: + """Carry a proven terminal endpoint into an immediate reverse pass.""" + carried = getattr(self, "carried_sweep_start_frames", None) + if carried is None: + self.carried_sweep_start_frames = [] + carried = self.carried_sweep_start_frames + carried.clear() + if ( + completed.spec != following.spec + or completed.target_u8 != following.start_u8 + or completed.direction == following.direction + ): + return 0 + tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, following.spec, following.start_u8 + ) + ) + limit_per_view = max(1, int(getattr(self, "preflight_frames", 30))) + selected: list[FrameObservation] = [] + for view in _sweep_views(_node_profile(self), following.spec): + view_frames = [ + frame + for frame in self.sweep_frames + if frame.view == view + and abs( + float(frame.state_u8[following.spec.motor_index]) + - following.start_u8 + ) + <= tolerance + ] + selected.extend(view_frames[-limit_per_view:]) + selected.sort(key=lambda frame: frame.stamp_ns) + carried.extend(selected) + if selected and getattr(self, "raw_path", None) is not None: + append_jsonl( + self.raw_path, + { + "kind": "sweep_endpoint_frames_carried", + "task_name": completed.spec.key, + "from_cycle": completed.cycle, + "from_direction": completed.direction, + "to_cycle": following.cycle, + "to_direction": following.direction, + "endpoint_u8": following.start_u8, + "valid_frames": len(selected), + "valid_frames_by_joint": { + name: len(_frames_for_joint(selected, name)) + for name in following.spec.joints + }, + }, + ) + return len(selected) + def _begin_active_sweep(self, now: float) -> None: assert self.active_sweep is not None self.sweep_frames.clear() @@ -5282,7 +6501,13 @@ class G20ThreeCameraCalibrationNode(Node): ) if ( profile.layout_id == G20_RIGHT_19_LAYOUT - and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + and ( + joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + or ( + joint_name.endswith("_mcp_roll") + and not joint_name.startswith("thumb_") + ) + ) ): return fit_joint_image_curve( model_records, @@ -5386,6 +6611,20 @@ class G20ThreeCameraCalibrationNode(Node): for finger in ("index", "middle", "ring", "pinky") }, }.get(joint_name) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and joint_name.endswith("_mcp_roll") + and not joint_name.endswith("_mcp_roll_side") + and not joint_name.startswith("thumb_") + ): + reference_roll = f"{profile.reference_finger}_mcp_roll" + if joint_name != reference_roll: + # The four finger roll axes are exactly parallel in the + # product URDF. The reference-finger task is acquired first, + # so reuse its measured common-frame direction for the other + # three fingers. This removes session-to-session planar-PnP + # branch tilt without changing any fitted axis-line position. + upstream_joint = reference_roll if upstream_joint is not None: # Resolve recursively so thumb_ip receives the already constrained # thumb_mcp direction (and index_dip the constrained index_pip @@ -5449,9 +6688,14 @@ class G20ThreeCameraCalibrationNode(Node): ) if extrinsics is None: return measurement - return replace( - measurement, - view_normal_common_xyz=tuple(float(value) for value in view_normal), + return with_depth_free_axis_projection( + replace( + measurement, + view_normal_common_xyz=tuple( + float(value) for value in view_normal + ), + ), + extrinsics.transform(spec.view)[:3, 3], ) def _fit_axis_measurement( @@ -5492,6 +6736,71 @@ class G20ThreeCameraCalibrationNode(Node): np.cross(secondary_point - primary_point, primary_axis) ) ) + validation_spec = profile.record_specs[validation_name] + if validation_spec.zero_kind == "axis_cross_view_validation": + # This Tag is attached to the downstream PIP link. During MCP + # roll its orientation contains linkage/pitch coupling and is not + # an independent observation of the published MCP angle or axis + # direction. It remains the physical axis-line-position source; + # its own radial, line-repeatability, visibility and trajectory + # quality gates are all applied before reaching this point. + decision = ( + "diagnostic_only_gross_pose_disagreement_use_primary" + if axis_difference + > getattr( + self, + "cross_view_roll_maximum_axis_difference_rad", + math.radians(15.0), + ) + else "diagnostic_only_pose_disagreement_use_primary" + ) + G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( + self, + joint_name, + cycle, + primary=primary, + secondary=secondary, + axis_difference_rad=axis_difference, + line_distance_m=line_distance, + decision=decision, + ) + extrinsics = getattr(self, "extrinsics", None) + if extrinsics is None or validation_spec.view is None: + raise ValueError( + f"{validation_name}: cross-view camera geometry is unavailable" + ) + primary_view = profile.record_specs[joint_name].view + if primary_view is None: + raise ValueError( + f"{joint_name}: primary point source has no view" + ) + point_camera_center = extrinsics.transform(primary_view)[:3, 3] + point_ray = primary_point - point_camera_center + interpretation_plane_normal = np.cross( + point_ray, primary_axis + ) + interpretation_plane_normal /= np.linalg.norm( + interpretation_plane_normal + ) + return replace( + primary, + point_common_xyz_m=tuple( + float(value) for value in primary.point_common_xyz_m + ), + axis_point_source=( + "front_interpretation_plane_cross_view_validated" + ), + pose_axis_line_rms_m=primary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(joint_name,), + axis_point_camera_center_common_xyz_m=tuple( + float(value) + for value in point_camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) + for value in interpretation_plane_normal + ), + ) # Monocular planar-tag orientation on the side view carries a # systematic line-of-sight IPPE bias of several degrees through roll # sweeps (tags tilted ~13-20 deg from the ray), which sub-pixel @@ -5500,22 +6809,21 @@ class G20ThreeCameraCalibrationNode(Node): # so sub-degree cross-view agreement is not achievable at this # geometry. Disagreement above the fusion gate now falls back to the # trusted front-only axis with a recorded diagnostic instead of - # failing the joint; only the gross bound (wrong-link or loose tag) - # still fails. + # failing the joint; only a gross direction error (wrong-link or a + # loose Tag that changes the observed motion axis) still fails here. gross_axis_limit = getattr( self, "cross_view_roll_maximum_axis_difference_rad", math.radians(15.0), ) - gross_line_limit = getattr( - self, - "cross_view_roll_maximum_axis_line_difference_m", - 0.030, - ) - if ( - axis_difference > gross_axis_limit - or line_distance > gross_line_limit - ): + # Do not apply a universal front/side line-distance gate here. The + # front roll link rides the splay screw, so its pure-revolute pose fit + # is a displaced pseudo-line whose offset varies by finger (about + # 15 mm for pinky and 37 mm for ring on the same fixed setup). It is + # not the physical line ultimately published. Retain the distance in + # diagnostics; validate the selected side physical line through its + # own radial quality, cycle RMS and final URDF geometry instead. + if axis_difference > gross_axis_limit: raise ValueError( "cross_view_roll_axis_gross_disagreement:" f"{math.degrees(axis_difference):.6f}deg," @@ -5541,15 +6849,7 @@ class G20ThreeCameraCalibrationNode(Node): if observer_name is None else self.records_by_joint.get(observer_name, []) ) - if any( - int(record.get("cycle", -1)) == int(cycle) - for record in observer_records - ): - observer = ( - G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, observer_name, cycle - ) - ) + if observer_records: model = UrdfKinematicModel(self.source_urdf_path) parent_axis, _ = model.axis_line( joint_name, zero_offsets={}, joint_angles={} @@ -5564,11 +6864,11 @@ class G20ThreeCameraCalibrationNode(Node): ) ) ) - measured_observer_axis = np.asarray( - observer.axis_common_xyz, dtype=float - ) - def cone_residual(candidate_axis: np.ndarray) -> float: + def cone_residual( + candidate_axis: np.ndarray, + measured_observer_axis: np.ndarray, + ) -> float: measured_cone = math.acos( abs( float( @@ -5582,19 +6882,128 @@ class G20ThreeCameraCalibrationNode(Node): ) return abs(measured_cone - expected_cone) - primary_cone_residual = cone_residual(primary_axis) - secondary_cone_residual = cone_residual(secondary_axis) # A zero rotates the downstream axis around this parent and - # cannot change their mutual cone angle. Use that invariant - # to choose between the two independently accepted views only - # when the current primary is invalid and the secondary is - # inside the unchanged geometry gate. - if ( - primary_cone_residual - > self.zero_maximum_axis_cone_mismatch_rad - and secondary_cone_residual - <= self.zero_maximum_axis_cone_mismatch_rad - ): + # cannot change their mutual cone angle. Select the camera + # once for the complete repeated-sweep group. Per-cycle + # selection is invalid because a residual close to the gate + # can alternate views and turn their fixed systematic bias + # into a false cycle-axis spread. + group_cycles = tuple(range(int(self.repetitions))) + required_names = ( + joint_name, + validation_name, + observer_name, + ) + group_is_complete = all( + any( + int(record.get("cycle", -1)) == group_cycle + for record in self.records_by_joint.get(name, ()) + ) + for name in required_names + for group_cycle in group_cycles + ) + group_source = "primary" + if group_is_complete: + group_signature = tuple( + ( + name, + group_cycle, + len(cycle_records), + max( + ( + int(record.get("attempt", 0)) + for record in cycle_records + ), + default=0, + ), + max( + ( + int(record.get("image_stamp_ns", 0)) + for record in cycle_records + ), + default=0, + ), + ) + for name in required_names + for group_cycle in group_cycles + for cycle_records in ( + [ + record + for record in self.records_by_joint.get( + name, () + ) + if int(record.get("cycle", -1)) + == group_cycle + ], + ) + ) + cache_key = ( + joint_name, + validation_name, + observer_name, + group_signature, + ) + source_cache = getattr( + self, "_cross_view_roll_group_source_cache", {} + ) + cached_source = source_cache.get(cache_key) + if cached_source is not None: + group_source = str(cached_source) + else: + primary_residuals: list[float] = [] + secondary_residuals: list[float] = [] + fit_raw = ( + G20ThreeCameraCalibrationNode + ._fit_axis_measurement_raw + ) + try: + for group_cycle in group_cycles: + group_primary = fit_raw( + self, joint_name, group_cycle + ) + group_secondary = fit_raw( + self, validation_name, group_cycle + ) + group_observer = fit_raw( + self, observer_name, group_cycle + ) + observer_axis_common = np.asarray( + group_observer.axis_common_xyz, + dtype=float, + ) + primary_residuals.append( + cone_residual( + np.asarray( + group_primary.axis_common_xyz, + dtype=float, + ), + observer_axis_common, + ) + ) + secondary_residuals.append( + cone_residual( + np.asarray( + group_secondary.axis_common_xyz, + dtype=float, + ), + observer_axis_common, + ) + ) + except (KeyError, ValueError): + primary_residuals = [] + secondary_residuals = [] + group_source = ( + select_cross_view_roll_direction_source( + primary_residuals, + secondary_residuals, + self.zero_maximum_axis_cone_mismatch_rad, + ) + ) + source_cache[cache_key] = group_source + if len(source_cache) > 32: + source_cache.pop(next(iter(source_cache))) + self._cross_view_roll_group_source_cache = source_cache + if group_source == "secondary": selected_direction = replace( primary, axis_common_xyz=tuple( @@ -5604,7 +7013,9 @@ class G20ThreeCameraCalibrationNode(Node): "cross_view_cone_selected_secondary" ), ) - decision = "use_secondary_zero_invariant_cone" + decision = ( + "use_secondary_zero_invariant_cone_consensus" + ) G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( self, joint_name, @@ -5619,8 +7030,9 @@ class G20ThreeCameraCalibrationNode(Node): # tracks the rotation faithfully, but its centre trajectory # carries the screw translation, displacing the fitted axis # line by ~21 mm from the finger's physical MCP axis. The side - # PIP-link circle recovers that physical line. Keep the trusted - # front direction and take the line position from the side view. + # PIP-link circle recovers that physical line. Keep the one + # direction selected consistently for all cycles and take the + # line position from the side view. return replace( selected_direction, point_common_xyz_m=tuple( @@ -5629,6 +7041,7 @@ class G20ThreeCameraCalibrationNode(Node): ), axis_point_source="side_circle_cross_view", pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(validation_name,), ) primary_variance = max( primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, @@ -5666,9 +7079,56 @@ class G20ThreeCameraCalibrationNode(Node): secondary.pose_axis_line_rms_m, line_distance, ), + pose_axis_line_source_joints=(joint_name, validation_name), axis_direction_source="cross_view_weighted_fusion", ) + def _refit_cross_view_axis_line_group( + self, + joint_name: str, + measurements: Sequence[JointAxisMeasurement], + ) -> list[JointAxisMeasurement]: + """Apply the repeated-cycle physical-radius constraint when eligible.""" + group = list(measurements) + sources = { + source + for measurement in group + if (source := cross_view_side_line_source(measurement)) is not None + } + if ( + len(group) != int(self.repetitions) + or len(sources) != 1 + or not all( + cross_view_side_line_source(measurement) is not None + for measurement in group + ) + or any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in group + ) + ): + return group + source = next(iter(sources)) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + source_spec = profile.record_specs[source] + source_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + source, + self.records_by_joint[source], + ) + return list( + refit_axis_line_group_with_shared_radius( + group, + source_records, + zero_command_u8=int( + self.baseline_command[source_spec.motor_index] + ), + canonical_zero_direction=canonical_zero_direction( + profile, source + ), + ) + ) + def _record_cross_view_roll_axis_diagnostic( self, joint_name: str, @@ -5710,7 +7170,9 @@ class G20ThreeCameraCalibrationNode(Node): if callable(logger_factory): selected_text = ( "using the side axis selected by the zero-invariant cone" - if decision == "use_secondary_zero_invariant_cone" + if decision.startswith( + "use_secondary_zero_invariant_cone" + ) else "keeping the front-only axis" ) logger_factory().warning( @@ -5760,10 +7222,25 @@ class G20ThreeCameraCalibrationNode(Node): and spec.key in set(getattr(self, "resumed_task_keys", ()) or ()) ) + dense_trajectory_coverage = bool( + profile.layout_id == G20_RIGHT_19_LAYOUT + and _records_have_formal_sweep_coverage( + self.records_by_joint, + view_joints, + repetitions=self.repetitions, + minimum_bins=int( + getattr(self, "minimum_sweep_bins", 32) + ), + maximum_bin_gap=int( + getattr(self, "maximum_bin_gap", 16) + ), + ) + ) if ( not imported_without_capture and valid_rate < self.minimum_detection_rate and not validation_only_view + and not dense_trajectory_coverage ): failures.append( { @@ -5820,7 +7297,7 @@ class G20ThreeCameraCalibrationNode(Node): } ) continue - if profile.layout_id == G20_RIGHT_19_LAYOUT: + if profile.supports("steady_command_checkpoints"): command_store = getattr( self, "command_records_by_joint", None ) @@ -5913,6 +7390,10 @@ class G20ThreeCameraCalibrationNode(Node): profile, joint_name ) if canonical_direction is not None: + validation_only_pose = ( + profile.record_specs[joint_name].zero_kind + == "axis_cross_view_validation" + ) branch_gap_range = ( maximum_baseline_hysteresis - min(baseline_hysteresis) @@ -5927,20 +7408,6 @@ class G20ThreeCameraCalibrationNode(Node): "directional_zero_maximum_branch_gap_range_rad", math.radians(0.3), ) - if ( - profile.record_specs[joint_name].zero_kind - == "axis_cross_view_validation" - ): - # The validation-only side angle curve rides on a - # near-grazing planar tag, so its cross-round backlash - # repeatability carries IPPE drift beyond the strict - # 0.3 deg production bound; the absolute gap limit - # below still applies unchanged. - branch_gap_range_limit = getattr( - self, - "cross_view_roll_alias_maximum_branch_gap_range_rad", - math.radians(0.5), - ) if maximum_baseline_hysteresis > branch_gap_limit: failures.append( { @@ -5962,7 +7429,10 @@ class G20ThreeCameraCalibrationNode(Node): ], } ) - if branch_gap_range > branch_gap_range_limit: + if ( + branch_gap_range > branch_gap_range_limit + and not validation_only_pose + ): failures.append( { "joint": joint_name, @@ -5982,6 +7452,39 @@ class G20ThreeCameraCalibrationNode(Node): ], } ) + elif ( + branch_gap_range > branch_gap_range_limit + and validation_only_pose + ): + # This side-view alias is never published as a joint + # angle. Its orientation is reconstructed from a + # near-grazing planar Tag, so IPPE pose drift can move + # the apparent branch gap between rounds even when + # the image trajectory and the authoritative front + # axis are repeatable. Keep the production threshold + # as an audit reference, but do not rescan identical + # motion based on a non-published pose component. + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": joint_name, + "metric": ( + "baseline_directional_gap_range_deg" + ), + "actual": round( + math.degrees(branch_gap_range), 6 + ), + "reference_limit": round( + math.degrees(branch_gap_range_limit), 6 + ), + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + "decision": "diagnostic_only", + }, + ) elif ( maximum_baseline_hysteresis > self.baseline_maximum_hysteresis_rad @@ -6075,7 +7578,7 @@ class G20ThreeCameraCalibrationNode(Node): "maximum", ), ) - if profile.layout_id != G20_RIGHT_19_LAYOUT: + if not profile.supports("directional_zero"): checks = checks + ( ( "hysteresis_deg", @@ -6104,6 +7607,7 @@ class G20ThreeCameraCalibrationNode(Node): cycle_travels: list[float] = [] cycle_axes: list[np.ndarray] = [] cycle_axis_sources: list[str] = [] + cycle_axis_measurements: list[JointAxisMeasurement] = [] for cycle in range(self.repetitions): cycle_records = [ record @@ -6133,17 +7637,32 @@ class G20ThreeCameraCalibrationNode(Node): try: axis = self._fit_axis_measurement(joint_name, cycle) except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "axis_fit", - "cycle": cycle + 1, - "reason": str(error), - } - ) + reason = str(error) + failure: dict[str, Any] = { + "joint": joint_name, + "metric": "axis_fit", + "cycle": cycle + 1, + "reason": reason, + } + if reason.startswith( + "cross_view_roll_axis_gross_disagreement:" + ): + validation_name = ( + profile.axis_validation_sources or {} + ).get(joint_name) + if validation_name in spec.joints: + # A gross direction disagreement does not identify + # which independently observed view is wrong. A + # retry must therefore reacquire both sources. + failure["quality_source_joints"] = [ + joint_name, + validation_name, + ] + failures.append(failure) continue cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) cycle_axis_sources.append(axis.axis_direction_source) + cycle_axis_measurements.append(axis) plane_limit = ( self.axis_maximum_plane_rms_m if joint_spec.active @@ -6158,10 +7677,15 @@ class G20ThreeCameraCalibrationNode(Node): ] if ( joint_spec.zero_kind != "axis_cross_view_validation" + and cross_view_side_line_source(axis) is None ): - # The validation-only side axis line is not published once - # fusion is skipped, so its pose-line RMS stays a recorded - # diagnostic instead of an admissibility gate. + # A cross-view fallback deliberately takes its direction + # from the front and its line position from the side. The + # side Tag's monocular orientation is not the published + # direction, so its per-frame ideal-revolute residual is + # not an accuracy measure for the combined line. Those + # lines are checked for independent-cycle position + # repeatability below, at this same threshold. axis_checks.append( ( "axis_pose_line_rms_mm", @@ -6200,16 +7724,117 @@ class G20ThreeCameraCalibrationNode(Node): # error is an admissibility check for constrained circles. for metric, actual, limit in axis_checks: if actual > limit: - failures.append( - { - "joint": joint_name, - "metric": metric, - "cycle": cycle + 1, - "actual": round(float(actual), 6), - "limit": round(float(limit), 6), - "comparison": "maximum", - } + failure_joint = joint_name + quality_sources: tuple[str, ...] = () + if metric == "axis_pose_line_rms_mm": + quality_sources = tuple( + str(source) + for source in getattr( + axis, + "pose_axis_line_source_joints", + (), + ) + if str(source) in spec.joints + ) + if len(quality_sources) == 1: + failure_joint = quality_sources[0] + failure = { + "joint": failure_joint, + "metric": metric, + "cycle": cycle + 1, + "actual": round(float(actual), 6), + "limit": round(float(limit), 6), + "comparison": "maximum", + } + if quality_sources: + failure["quality_source_joints"] = list( + quality_sources + ) + if failure_joint != joint_name: + failure["model_joint"] = joint_name + failures.append(failure) + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if side_line_sources: + try: + cycle_axis_measurements = ( + G20ThreeCameraCalibrationNode + ._refit_cross_view_axis_line_group( + self, joint_name, cycle_axis_measurements ) + ) + except Exception as error: + source = next(iter(side_line_sources)) + failures.append( + { + "joint": source, + "model_joint": joint_name, + "quality_source_joints": [source], + "metric": "axis_line_shared_radius_fit", + "reason": str(error), + } + ) + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if ( + len(cycle_axis_measurements) == self.repetitions + and len(side_line_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in cycle_axis_measurements + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in cycle_axis_measurements + ) + ): + line_rms = axis_line_cycle_rms_m( + cycle_axis_measurements + ) + if line_rms > self.axis_maximum_pose_line_rms_m: + source = next(iter(side_line_sources)) + outliers = _isolated_axis_line_cycle_outliers( + cycle_axis_measurements, + self.axis_maximum_pose_line_rms_m, + ) + failure = { + "joint": source, + "model_joint": joint_name, + "quality_source_joints": [source], + "metric": "axis_line_cycle_rms_mm", + "actual": round(1000.0 * line_rms, 6), + "limit": round( + 1000.0 * self.axis_maximum_pose_line_rms_m, + 6, + ), + "maximum_pairwise_mm": round( + 1000.0 + * maximum_axis_line_cycle_spread_m( + cycle_axis_measurements + ), + 6, + ), + "comparison": "maximum", + } + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_axis_measurements)) + if index != outlier + ] + failures.append(failure) if len(cycle_travels) == self.repetitions: travel_range = max(cycle_travels) - min(cycle_travels) cycle_limit = ( @@ -6247,7 +7872,38 @@ class G20ThreeCameraCalibrationNode(Node): abs(float(np.clip(left @ right, -1.0, 1.0))) ), ) - if maximum_difference > self.zero_maximum_axis_cycle_difference_rad: + if ( + maximum_difference + > self.zero_maximum_axis_cycle_difference_rad + and joint_spec.zero_kind + == "axis_cross_view_validation" + ): + # As above, the side alias' independently reconstructed + # pose axis is not the axis written to the URDF. The + # published front direction and the side translation + # circle retain their own strict cross-round gates. + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": joint_name, + "metric": "axis_cycle_difference_deg", + "actual": round( + math.degrees(maximum_difference), 6 + ), + "reference_limit": round( + math.degrees( + self.zero_maximum_axis_cycle_difference_rad + ), + 6, + ), + "decision": "diagnostic_only", + }, + ) + elif ( + maximum_difference + > self.zero_maximum_axis_cycle_difference_rad + ): outliers = _isolated_axis_cycle_outliers( cycle_axes, self.zero_maximum_axis_cycle_difference_rad, @@ -6273,6 +7929,112 @@ class G20ThreeCameraCalibrationNode(Node): if index != outlier ] failures.append(failure) + if profile.supports("cross_view_roll_curve"): + for primary_name, validation_name in ( + profile.axis_validation_sources or {} + ).items(): + if not {primary_name, validation_name}.issubset(spec.joints): + continue + try: + cycle_fits = [ + ( + self._fit_joint_records( + primary_name, + [ + record + for record in self.records_by_joint[ + primary_name + ] + if int(record.get("cycle", -1)) == cycle + ], + ), + self._fit_joint_records( + validation_name, + [ + record + for record in self.records_by_joint[ + validation_name + ] + if int(record.get("cycle", -1)) == cycle + ], + ), + ) + for cycle in range(self.repetitions) + ] + training_cycles = set(range(max(1, self.repetitions - 1))) + scopes = ( + ( + "training", + [ + record + for record in self.records_by_joint[primary_name] + if int(record.get("cycle", -1)) + in training_cycles + ], + [ + record + for record in self.records_by_joint[ + validation_name + ] + if int(record.get("cycle", -1)) + in training_cycles + ], + ), + ( + "all_cycles", + self.records_by_joint[primary_name], + self.records_by_joint[validation_name], + ), + ) + for scope, primary_records, validation_records in scopes: + failure = _cross_view_curve_failure( + primary_name, + validation_name, + self._fit_joint_records( + primary_name, primary_records + ), + self._fit_joint_records( + validation_name, validation_records + ), + scope=scope, + maximum_rms_difference_rad=( + self.cross_view_roll_maximum_shape_rms_rad + ), + maximum_branch_gap_difference_rad=( + self.cross_view_roll_maximum_branch_gap_difference_rad + ), + allow_projection_scale=True, + maximum_projection_scale_ratio=( + self.cross_view_roll_maximum_projection_scale_ratio + ), + cycle_fits=cycle_fits, + ) + if failure is not None: + append_jsonl( + self.raw_path, + { + "kind": ( + "validation_only_quality_diagnostic" + ), + **failure, + "decision": "diagnostic_only", + }, + ) + # One structured result contains all per-cycle + # evidence; avoid duplicate training/final errors. + break + except Exception as error: + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": primary_name, + "metric": "cross_view_roll_curve", + "reason": str(error), + "quality_source_joints": [validation_name], + "decision": "diagnostic_only", + }, + ) return failures def _cross_view_roll_diagnostic_role( @@ -6476,6 +8238,9 @@ class G20ThreeCameraCalibrationNode(Node): _fit_failure_is_systematic(failures, self.repetitions) or repeated_branch_clusters ) + fit_retry_limit = int( + getattr(self, "automatic_fit_retry_limit", 0) + ) self.fit_failure = { "kind": "fit_failure", "view": spec.view, @@ -6484,6 +8249,10 @@ class G20ThreeCameraCalibrationNode(Node): "joints": list(spec.joints), "joints_to_rescan": sorted(self.retry_joint_names), "attempt": attempt, + "fit_attempt": attempt, + "fit_attempt_limit": fit_retry_limit + 1, + "automatic_retry_count": max(0, attempt - 1), + "automatic_retry_limit": fit_retry_limit, "cycles_to_rescan": [ cycle + 1 for cycle in sorted(self.retry_cycles) ], @@ -6508,7 +8277,7 @@ class G20ThreeCameraCalibrationNode(Node): else "joint_fit_systematic_failure" ) return True - if attempt <= getattr(self, "automatic_fit_retry_limit", 0): + if attempt <= fit_retry_limit: self.paused_reason = "joint_fit_check_failed" self.reason = "automatic_retry_joint_fit_check_failed" self._prepare_failed_sweep_retry() @@ -6689,7 +8458,18 @@ class G20ThreeCameraCalibrationNode(Node): return retries += 1 self.sweep_retry_counts[key] = retries - for view in _sweep_views(_node_profile(self), item.spec): + observer = next( + ( + candidate + for candidate in _node_profile(self).palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + retry_views = set(_sweep_views(_node_profile(self), item.spec)) + if observer is not None: + retry_views.add(observer.view) + for view in retry_views: runtime = getattr(self, "views", {}).get(view) if runtime is None: continue @@ -6707,6 +8487,18 @@ class G20ThreeCameraCalibrationNode(Node): and str(record.get("direction")) == item.direction ) ] + if observer is not None: + records = self.palm_axis_records_by_source.get( + observer.source_name, [] + ) + records[:] = [ + record + for record in records + if not ( + int(record.get("cycle", -999)) == item.cycle + and str(record.get("direction", "")) == item.direction + ) + ] getattr(self, "sweep_frames", []).clear() getattr(self, "sweep_start_frames", []).clear() speed_scales = getattr( @@ -6893,11 +8685,15 @@ class G20ThreeCameraCalibrationNode(Node): assert self.active_sweep is not None item = self.active_sweep motor = item.spec.motor_index - start_tolerance = self._endpoint_tolerance_for_spec( - item.spec, item.start_u8 + start_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, item.spec, item.start_u8 + ) ) - target_tolerance = self._endpoint_tolerance_for_spec( - item.spec, item.target_u8 + target_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, item.spec, item.target_u8 + ) ) record_joint_names = tuple(item.spec.joints) if getattr(self, "active_sweep_is_fit_retry", False): @@ -6906,10 +8702,13 @@ class G20ThreeCameraCalibrationNode(Node): record_joint_names = tuple( name for name in item.spec.joints if name in scoped ) - joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} - for joint_name in record_joint_names: + + def observation_bins( + joint_name: str, + ) -> dict[int, list[FrameObservation]]: bins: dict[int, list[FrameObservation]] = {} - for frame in _frames_for_joint(self.sweep_frames, joint_name): + frames = _frames_for_joint(self.sweep_frames, joint_name) + for frame in frames: state = float(frame.state_u8[motor]) if abs(state - item.start_u8) <= start_tolerance: command = item.start_u8 @@ -6918,6 +8717,11 @@ class G20ThreeCameraCalibrationNode(Node): else: command = int(np.clip(np.rint(state), 0, 255)) bins.setdefault(command, []).append(frame) + return bins + + joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} + for joint_name in record_joint_names: + bins = observation_bins(joint_name) commands = sorted(bins) if not commands or commands[0] != 0 or commands[-1] != 255: self._retry_active_sweep_or_pause( @@ -6930,8 +8734,11 @@ class G20ThreeCameraCalibrationNode(Node): if item.precheck: # This low-speed pass is a visibility/safety check at 0, 127 and # 255, not a source for the command-angle curve. Its full-sweep - # detection rate is counted directly in the camera callback; - # dense feedback-bin coverage remains a formal-curve requirement. + # detection rate is counted directly in the camera callback. A + # raw-frame rate below the nominal threshold is not itself a data + # defect when synchronised valid poses still cover the complete + # trajectory densely: detector flicker then changes only how many + # duplicate observations exist, not what motion is observable. for joint_name, bins in joint_bins.items(): commands = sorted(bins) if min(abs(command - 127) for command in commands) > 2: @@ -6951,6 +8758,24 @@ class G20ThreeCameraCalibrationNode(Node): self, "sweep_detection_valid_by_view", {} ) capture_views = _sweep_views(_node_profile(self), item.spec) + profile = _node_profile(self) + trajectory_coverage_by_view: dict[str, bool] = {} + for view in capture_views: + view_joint_bins = [ + joint_bins[name] + for name in _sweep_joints_for_view( + profile, item.spec, view + ) + if name in joint_bins + ] + trajectory_coverage_by_view[view] = bool(view_joint_bins) and all( + len(bins) >= self.minimum_sweep_bins + and max(np.diff(sorted(bins)), default=0) + <= self.maximum_bin_gap + for bins in view_joint_bins + ) + detection_rate_below_threshold_views: list[str] = [] + trajectory_coverage_override_views: list[str] = [] for view in capture_views: if view in total_by_view: total = int(total_by_view.get(view, 0)) @@ -6970,10 +8795,14 @@ class G20ThreeCameraCalibrationNode(Node): rate = 0.0 if total <= 0 else valid / total detection_rates[view] = rate if rate < self.minimum_detection_rate: - self._retry_active_sweep_or_pause( - f"task_precheck_detection_rate_too_low:{view}" - ) - return + detection_rate_below_threshold_views.append(view) + if trajectory_coverage_by_view.get(view, False): + trajectory_coverage_override_views.append(view) + else: + self._retry_active_sweep_or_pause( + f"task_precheck_detection_rate_too_low:{view}" + ) + return minimum_bin_count = min( len(bins) for bins in joint_bins.values() ) @@ -7011,6 +8840,18 @@ class G20ThreeCameraCalibrationNode(Node): view: round(rate, 6) for view, rate in detection_rates.items() }, + "minimum_detection_rate": round( + float(self.minimum_detection_rate), 6 + ), + "detection_rate_below_threshold_views": ( + detection_rate_below_threshold_views + ), + "trajectory_coverage_by_view": ( + trajectory_coverage_by_view + ), + "trajectory_coverage_override_views": ( + trajectory_coverage_override_views + ), "passed": True, }, ) @@ -7024,12 +8865,18 @@ class G20ThreeCameraCalibrationNode(Node): if self.sweep_index >= len(self.sweep_items): self._begin_return_baseline("fit") elif self.sweep_items[self.sweep_index].spec == item.spec: - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and item.direction == DIRECTION_INCREASING - ): + following = self.sweep_items[self.sweep_index] + transition = _sweep_plan_transition( + self.profile, item, following + ) + if transition == "cycle_reset": self._begin_return_baseline("next_cycle") else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + following, + ) self._start_next_sweep() else: self._begin_return_baseline( @@ -7056,12 +8903,14 @@ class G20ThreeCameraCalibrationNode(Node): ) return + stored_joint_names = tuple(joint_bins) + def synchronised_frame_payload( frame: FrameObservation, ) -> dict[str, Any]: names = [ name - for name in record_joint_names + for name in stored_joint_names if name in frame.joint_quaternions_xyzw ] return { @@ -7112,12 +8961,12 @@ class G20ThreeCameraCalibrationNode(Node): for frame in self.sweep_frames if any( name in frame.joint_quaternions_xyzw - for name in record_joint_names + for name in stored_joint_names ) ), ) - for joint_name in record_joint_names: + for joint_name in stored_joint_names: bins = joint_bins[joint_name] commands = sorted(bins) for command in commands: @@ -7222,6 +9071,7 @@ class G20ThreeCameraCalibrationNode(Node): ) append_jsonl(self.raw_path, durable) + G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item) G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold( self, item ) @@ -7236,13 +9086,18 @@ class G20ThreeCameraCalibrationNode(Node): self.active_sweep_is_fit_retry = False if was_fit_retry: if self.retry_sweep_items: - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and item.direction == DIRECTION_INCREASING - and self.retry_sweep_items[0].spec == previous_spec - ): + following = self.retry_sweep_items[0] + transition = _sweep_plan_transition( + self.profile, item, following + ) + if transition == "cycle_reset": self._begin_return_baseline("next_cycle") else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + following, + ) self._start_next_sweep() return failures = self._provisional_fit_failures(previous_spec) @@ -7311,19 +9166,26 @@ class G20ThreeCameraCalibrationNode(Node): self, previous_spec ) ) - elif ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and item.direction == DIRECTION_INCREASING - ): + elif _sweep_plan_transition( + self.profile, + item, + self.sweep_items[self.sweep_index], + ) == "cycle_reset": # Keep the task's complete avoidance pose between its three - # rounds. Only the active joint returns to its standard-side + # repetitions. Only the active joint returns to its standard-side # baseline (roll: 255->127); global unfolding is deferred until # the task really changes. self._begin_return_baseline("next_cycle") else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + self.sweep_items[self.sweep_index], + ) self._start_next_sweep() def _fit_all_curves(self) -> None: + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) self.state = STATE_FITTING self.reason = "fitting_3d_axes_and_urdf_zero_offsets" # Fitting is deliberately synchronous so no capture callback can @@ -7485,40 +9347,18 @@ class G20ThreeCameraCalibrationNode(Node): self.validation_only_fits[validation_name] = ( validation_training_fit ) - try: - training_metrics = compare_cross_view_roll_curves( - training_fits[name], - validation_training_fit, - maximum_rms_difference_rad=self.maximum_validation_mae_rad, - maximum_branch_gap_difference_rad=( - self.cross_view_roll_maximum_branch_gap_difference_rad - ), - ) - final_metrics = compare_cross_view_roll_curves( - measured[name], - validation_fit, - maximum_rms_difference_rad=self.maximum_validation_mae_rad, - maximum_branch_gap_difference_rad=( - self.cross_view_roll_maximum_branch_gap_difference_rad - ), - ) - except ValueError as error: - failed_spec = next( - spec - for spec in self.profile.sweep_specs - if validation_name in spec.joints - ) - self._pause_for_provisional_fit_failure( - failed_spec, - [ - { - "joint": name, - "metric": "cross_view_roll_curve", - "reason": str(error), - } - ], - ) - return + # The structured task-level check above records threshold + # overruns with per-cycle evidence. At publication time this + # secondary view remains diagnostic: the primary front curve and + # the side physical axis line retain their independent hard + # quality gates. Reuse the same policy as offline replay instead + # of reintroducing an unlocalized retry from a second code path. + training_metrics = cross_view_roll_diagnostic_metrics( + training_fits[name], validation_training_fit + ) + final_metrics = cross_view_roll_diagnostic_metrics( + measured[name], validation_fit + ) validation_errors = joint_curve_holdout_errors( validation_training_fit, validation_holdout_model_records, @@ -7595,25 +9435,90 @@ class G20ThreeCameraCalibrationNode(Node): } axes: list[JointAxisMeasurement] = [] for name in self.zero_profile.axis_joints: - for cycle in range(self.repetitions): - axes.append(self._fit_axis_measurement(name, cycle)) + group = [ + self._fit_axis_measurement(name, cycle) + for cycle in range(self.repetitions) + ] + axes.extend(self._refit_cross_view_axis_line_group(name, group)) + + try: + palm_orientation_measurements, palm_orientation_rejections = ( + fit_partial_palm_orientation_measurements( + sources=self.profile.palm_orientation_sources, + records_by_joint=self.palm_axis_records_by_source, + motor_by_source=self.profile.palm_axis_motor_by_source, + baseline_command_u8=self.baseline_command, + cycles=range(self.repetitions), + minimum_sources=int( + self.profile.minimum_palm_orientation_sources + ), + minimum_arc_rad=self.trajectory_minimum_arc_rad, + maximum_rotation_orthogonal_rms_rad=( + self.active_maximum_rotation_orthogonal_rms_rad + ), + ) + ) + except ValueError as error: + failure = str(error) + self.palm_orientation_rejections = {"coverage": failure} + self.fit_failure = { + "task": "palm_orientation_side_channel", + "failures": [ + { + "joint": "palm_orientation", + "metric": "palm_orientation_coverage", + "reason": failure, + } + ], + "directions_to_rescan": 0, + } + append_jsonl( + self.raw_path, + { + "kind": "palm_orientation_optional_source_diagnostic", + "rejections": {"coverage": failure}, + "decision": "reject_unobservable_palm_phase", + }, + ) + self._pause("palm_orientation_quality_failed") + return + if palm_orientation_rejections: + append_jsonl( + self.raw_path, + { + "kind": "palm_orientation_optional_source_diagnostic", + "rejections": dict(palm_orientation_rejections), + "decision": "use_qualified_redundant_sources", + }, + ) motor_by_joint = { name: int(spec.motor_index) for name, spec in self.profile.joint_specs.items() } - endpoint_zero_offsets = ( - derive_right_19_flexion_endpoint_offsets( - self.source_urdf_path, + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + command_fits = anchor_right_19_mechanical_endpoint_curves( command_fits, - maximum_offset_rad=self.flexion_endpoint_maximum_offset_rad, + self.command_records_by_joint, + maximum_direction_difference_rad=( + self.command_maximum_direction_gap_rad + ), ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else {} - ) + endpoint_zero_offsets = ( + derive_right_19_mechanical_endpoint_offsets( + self.source_urdf_path, + command_fits, + maximum_offset_rad=( + self.mechanical_endpoint_maximum_offset_rad + ), + ) + ) + else: + endpoint_zero_offsets = {} holdout_zero_result = solve_urdf_zero_offsets( source_urdf=self.source_urdf_path, measurements=axes, + palm_orientation_measurements=palm_orientation_measurements, curves=training_fits, motor_by_joint=motor_by_joint, maximum_offset_rad=self.zero_maximum_offset_rad, @@ -7633,6 +9538,9 @@ class G20ThreeCameraCalibrationNode(Node): maximum_axis_cone_mismatch_rad=( self.zero_maximum_axis_cone_mismatch_rad ), + maximum_systematic_axis_cone_bias_rad=( + self.cross_view_roll_maximum_axis_difference_rad + ), maximum_observability_condition_number=( self.zero_maximum_observability_condition_number ), @@ -7706,6 +9614,13 @@ class G20ThreeCameraCalibrationNode(Node): "failure_reasons": dict( holdout_zero_result.failure_reasons ), + "axis_cone_mismatch_by_joint_rad": dict( + holdout_zero_result.axis_cone_mismatch_by_joint_rad + ), + "axis_cone_bias_classification_by_joint": dict( + holdout_zero_result + .axis_cone_bias_classification_by_joint + ), }, ) holdout_errors = np.abs( @@ -7862,6 +9777,12 @@ class G20ThreeCameraCalibrationNode(Node): "offset_covariance_rad2": dict( zero_result.offset_covariance_rad2 ), + "axis_cone_mismatch_by_joint_rad": dict( + zero_result.axis_cone_mismatch_by_joint_rad + ), + "axis_cone_bias_classification_by_joint": dict( + zero_result.axis_cone_bias_classification_by_joint + ), }, ) # Runtime/MuJoCo consumes requested commands, never feedback bins. @@ -7877,7 +9798,14 @@ class G20ThreeCameraCalibrationNode(Node): endpoint_anchored_offsets_rad=endpoint_zero_offsets, ) self.axis_measurements = axes + self.palm_orientation_measurements = list( + palm_orientation_measurements + ) + self.palm_orientation_rejections = dict( + palm_orientation_rejections + ) self.zero_result = zero_result + self.validated_endpoint_zero_offsets_rad = dict(endpoint_zero_offsets) self.validation_errors_rad = [ float(value) for values in holdout_by_joint.values() @@ -8403,6 +10331,10 @@ class G20ThreeCameraCalibrationNode(Node): passed = bool(self.fit_quality_passed and validation_passed) if not passed: raise RuntimeError("final_calibration_quality_failed") + endpoint_zero_offsets = ( + G20ThreeCameraCalibrationNode + ._endpoint_zero_offsets_for_publication(self) + ) stamp = ( self.session_dir.name if re.fullmatch(r"\d{8}_\d{6}", self.session_dir.name) @@ -8488,6 +10420,29 @@ class G20ThreeCameraCalibrationNode(Node): "offset_covariance_rad2": ( self.zero_result.offset_covariance_rad2 ), + "palm_orientation_sources": [ + { + "source_joint": item.source_joint, + "model_joint": item.model_joint, + "cycle": item.cycle, + "observed_arc_rad": item.observed_arc_rad, + "rotation_orthogonal_rms_rad": ( + item.rotation_orthogonal_rms_rad + ), + } + for item in getattr( + self, + "palm_orientation_measurements", + (), + ) + ], + "palm_orientation_rejections": dict( + getattr( + self, + "palm_orientation_rejections", + {}, + ) + ), } if self.profile.layout_id == G20_RIGHT_19_LAYOUT else None @@ -8526,6 +10481,48 @@ class G20ThreeCameraCalibrationNode(Node): return retries += 1 self.motion_retry_counts[reason] = retries + reset_views: list[str] = [] + task_reference_preserved = False + if ( + reason == "sweep_start_tag_timeout" + and self.state == STATE_PREPARE_SWEEP + and self.active_sweep is not None + ): + # Re-publishing the same endpoint command cannot repair a PnP + # tracker stuck on a planar mirror branch. Give every active + # camera a genuinely fresh bounded initialization window while + # retaining the task-level endpoint reference established by the + # visibility precheck/earlier cycles. + profile = _node_profile(self) + task_reference_preserved = ( + _preserve_pnp_task_reference_for_sweep( + self.active_sweep, + is_fit_retry=bool( + getattr(self, "active_sweep_is_fit_retry", False) + ), + has_precheck_anchor=( + profile.layout_id == G20_RIGHT_19_LAYOUT + ), + ) + ) + reset_trackers = getattr(self, "_reset_view_trackers", None) + reset_diagnostics = getattr( + self, "_reset_view_pnp_diagnostics", None + ) + for view in _sweep_views(profile, self.active_sweep.spec): + runtime = getattr(self, "views", {}).get(view) + if runtime is None or not callable(reset_trackers): + continue + reset_trackers( + runtime, + preserve_task_reference=task_reference_preserved, + ) + if callable(reset_diagnostics): + reset_diagnostics(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + reset_views.append(view) + getattr(self, "sweep_start_frames", []).clear() append_jsonl( self.raw_path, { @@ -8534,6 +10531,8 @@ class G20ThreeCameraCalibrationNode(Node): "reason": reason, "retry": retries, "retry_limit": self.automatic_motion_retry_limit, + "pnp_trackers_reset": reset_views, + "task_reference_preserved": task_reference_preserved, }, ) self.motion_stage_started_at = now @@ -8837,6 +10836,9 @@ class G20ThreeCameraCalibrationNode(Node): self.motion_retry_counts.pop( "sweep_start_position_timeout", None ) + self.motion_retry_counts.pop( + "sweep_start_tag_timeout", None + ) self.position_hold_since = None self._begin_active_sweep(now) else: @@ -9186,6 +11188,34 @@ class G20ThreeCameraCalibrationNode(Node): ), "pnp_rejections": dict(runtime.latest_pnp_rejections), "group_pnp_reason": runtime.latest_group_pnp_reason, + "group_missing_candidate_roles": list( + runtime.latest_group_missing_candidate_roles + ), + "pnp_candidate_diagnostics": { + role: { + "tag_id": runtime.view_tags[role], + **dict(diagnostics), + } + for role, diagnostics in ( + runtime.tracker.last_candidate_diagnostics_by_role.items() + ) + if role in required_roles and role not in locked_roles + }, + "pnp_pose_valid": runtime.latest_pnp_valid, + "pnp_rejection_counts": dict( + runtime.pnp_rejection_counts + ), + "group_pnp_rejection_counts": dict( + runtime.group_pnp_rejection_counts + ), + "pnp_initialization_progress": ( + None + if runtime.pnp_initialization_progress is None + else { + "accepted": runtime.pnp_initialization_progress[0], + "required": runtime.pnp_initialization_progress[1], + } + ), "pnp_reset_count": runtime.pnp_reset_count, "pnp_invalid_seconds": ( 0.0 @@ -9198,10 +11228,24 @@ class G20ThreeCameraCalibrationNode(Node): active = dict(self.motion_stall_details) elif self.active_sweep is not None: motor = self.active_sweep.spec.motor_index - start_tolerance = self._endpoint_tolerance_for_spec( + start_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, + self.active_sweep.spec, + self.active_sweep.start_u8, + ) + ) + target_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, + self.active_sweep.spec, + self.active_sweep.target_u8, + ) + ) + mechanical_start_tolerance = self._endpoint_tolerance_for_spec( self.active_sweep.spec, self.active_sweep.start_u8 ) - target_tolerance = self._endpoint_tolerance_for_spec( + mechanical_target_tolerance = self._endpoint_tolerance_for_spec( self.active_sweep.spec, self.active_sweep.target_u8 ) values = [ @@ -9287,13 +11331,31 @@ class G20ThreeCameraCalibrationNode(Node): storage_key, 1 ), "fit_attempt_limit": self.automatic_fit_retry_limit + 1, - "automatic_retry_count": self.sweep_retry_counts.get( - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, + "fit_retry_cycles": ( + [ + cycle + 1 + for cycle in sorted( + getattr(self, "retry_cycles", set()) + ) + ] + if getattr(self, "active_sweep_is_fit_retry", False) + else [] + ), + "automatic_retry_count": max( + self.sweep_retry_counts.get( + ( + storage_key, + self.active_sweep.cycle, + self.active_sweep.direction, + ), + 0, + ), + self.motion_retry_counts.get( + "sweep_start_tag_timeout", 0 + ), + self.motion_retry_counts.get( + "sweep_start_position_timeout", 0 ), - 0, ), "automatic_retry_limit": self.automatic_sweep_retry_limit, "retry_speed_scale": ( @@ -9397,6 +11459,17 @@ class G20ThreeCameraCalibrationNode(Node): ), "start_endpoint_tolerance_u8": start_tolerance, "target_endpoint_tolerance_u8": target_tolerance, + "mechanical_endpoint_tolerance_u8": max( + mechanical_start_tolerance, + mechanical_target_tolerance, + ), + "synchronised_endpoint_tolerance_margin_u8": float( + getattr( + self, + "synchronised_endpoint_tolerance_margin_u8", + 1.0, + ) + ), }, "auxiliary_motors": [ { @@ -9479,6 +11552,37 @@ class G20ThreeCameraCalibrationNode(Node): if group_pnp_reasons: active = dict(active) active["group_pnp_reasons"] = group_pnp_reasons + pnp_rejection_counts = { + name: dict(runtime.pnp_rejection_counts) + for name, runtime in self.views.items() + if runtime.pnp_rejection_counts + } + if pnp_rejection_counts: + active = dict(active) + active["pnp_rejection_counts"] = pnp_rejection_counts + group_pnp_rejection_counts = { + name: dict(runtime.group_pnp_rejection_counts) + for name, runtime in self.views.items() + if runtime.group_pnp_rejection_counts + } + if group_pnp_rejection_counts: + active = dict(active) + active["group_pnp_rejection_counts"] = ( + group_pnp_rejection_counts + ) + pnp_initialization_progress = { + name: { + "accepted": runtime.pnp_initialization_progress[0], + "required": runtime.pnp_initialization_progress[1], + } + for name, runtime in self.views.items() + if runtime.pnp_initialization_progress is not None + } + if pnp_initialization_progress: + active = dict(active) + active["pnp_initialization_progress"] = ( + pnp_initialization_progress + ) resumed_tasks = set(self.resumed_task_keys) completed_sweep_count = sum( 1 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py index 5485704..56b94d1 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py @@ -2,7 +2,7 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, replace from datetime import datetime import math import os @@ -28,7 +28,12 @@ from .full_hand import ( JointCurveFit, get_hand_calibration_profile, ) -from .trajectory import _fit_circle_with_axis, _fit_joint_curve, _fit_plane_axis +from .trajectory import ( + _fit_circle_with_axis, + _fit_joint_curve, + _fit_plane_axis, + _plane_basis, +) @dataclass(frozen=True) @@ -143,9 +148,11 @@ def _build_right_19_zero_profile( Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but repeated motion with one unchanged camera/Tag installation cannot by itself distinguish a fixed parallel-axis phase bias from an encoder zero. - The caller therefore anchors MCP-pitch and PIP to the independently known - full-flexion contact endpoint using their measured command curves. DIP - stays passive and has no independently identifiable static zero. + The three coupled thumb-CMC offsets must therefore remain a visual + multi-axis solve; their electrical endpoints are not assumed to coincide + with source-CAD limits. Independently repeatable contact endpoints still + anchor thumb MCP, finger MCP-pitch and PIP from each session. DIP stays + passive and has no independently identifiable static zero. """ fingers = ("index", "middle", "ring", "pinky") direct = ( @@ -236,41 +243,179 @@ def _build_right_19_zero_profile( axis_parent_joint=axis_parent, phase_parent_joint=phase_parent, offset_observer_joint=observer, - # thumb_mcp retains the commit-proven CAD reference. Four-finger - # flexion offsets are supplied per session from the measured endpoint, - # not stored as constants in this profile. - fixed_direct_zero_offsets_rad={"thumb_mcp": 0.0}, + # Endpoint-observable offsets are supplied by the caller. CMC remains + # visually solved and no serial-specific zero is hidden in the shared + # profile. + fixed_direct_zero_offsets_rad={}, static_output_zero_offsets_rad={}, ) -RIGHT_19_FLEXION_ENDPOINT_JOINTS = frozenset( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") +RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( + { + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), + } ) -def derive_right_19_flexion_endpoint_offsets( +def anchor_right_19_mechanical_endpoint_curves( + curves: Mapping[str, JointCurveFit], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + *, + maximum_direction_difference_rad: float = math.radians(1.0), + maximum_curve_correction_rad: float = math.radians(3.0), +) -> dict[str, JointCurveFit]: + """Set mechanical-endpoint curve scale from direct SO(3) travel. + + A fitted-axis projection is useful for a dense, signed command curve, but + a small view-dependent orthogonal PnP component can change that + projection's full-scale value. The rotation magnitude between the two + settled endpoint poses is the revolute-joint travel itself: conjugating + both poses by a rigid hand/camera transform or either fixed Tag mounting + rotation cannot change it. + + Use both sweep directions as independent endpoint measurements, reject + disagreement, and apply their robust mean as one scale correction to the + already validated curve. Curve shape, hysteresis, axis diagnostics and + all existing quality gates remain unchanged. + """ + if ( + not math.isfinite(maximum_direction_difference_rad) + or maximum_direction_difference_rad <= 0.0 + ): + raise ValueError("endpoint direction difference limit must be positive") + if ( + not math.isfinite(maximum_curve_correction_rad) + or maximum_curve_correction_rad <= 0.0 + ): + raise ValueError("endpoint curve correction limit must be positive") + + result = dict(curves) + for name in sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS): + fit = curves.get(name) + records = list(records_by_joint.get(name, ())) + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + + direction_travel: dict[str, float] = {} + for direction in ("decreasing", "increasing"): + endpoint_rotations: dict[int, Rotation] = {} + for command in (0, 255): + quaternions = [ + _relative_rotation(record) + for record in records + if str(record.get("direction")) == direction + and int( + record.get( + "requested_command_u8", + record.get("command_u8", -1), + ) + ) + == command + ] + if not quaternions: + raise ValueError( + f"{name} {direction} is missing settled endpoint {command}" + ) + endpoint_rotations[command] = Rotation.from_quat( + robust_rotation_summary(quaternions)[0] + ) + direction_travel[direction] = float( + ( + endpoint_rotations[255].inv() + * endpoint_rotations[0] + ).magnitude() + ) + + travel_values = np.asarray(list(direction_travel.values()), dtype=float) + direction_difference = float(np.ptp(travel_values)) + if direction_difference > maximum_direction_difference_rad: + raise ValueError( + f"{name} settled endpoint directions disagree: " + f"{math.degrees(direction_difference):.3f}deg" + ) + direct_travel = float(np.median(travel_values)) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + projected_travel = float(curve[0] - curve[255]) + if direct_travel <= 0.0 or projected_travel <= 0.0: + raise ValueError(f"{name} endpoint travel must be positive") + correction = abs(projected_travel - direct_travel) + if correction > maximum_curve_correction_rad: + raise ValueError( + f"{name} fitted/direct endpoint travel differs by " + f"{math.degrees(correction):.3f}deg" + ) + scale = direct_travel / projected_travel + + def scaled(values: Sequence[float]) -> tuple[float, ...]: + return tuple(float(value) * scale for value in values) + + circle = dict(fit.circle) + circle.update( + { + "mechanical_endpoint_direct_travel_rad": direct_travel, + "mechanical_endpoint_direction_difference_rad": ( + direction_difference + ), + "mechanical_endpoint_raw_curve_travel_rad": projected_travel, + "mechanical_endpoint_curve_scale": scale, + } + ) + quality = dict(fit.quality) + if "arc_rad" in quality: + quality["arc_rad"] = float(quality["arc_rad"]) * scale + result[name] = replace( + fit, + angle_rad=scaled(fit.angle_rad), + decreasing_rad=scaled(fit.decreasing_rad), + increasing_rad=scaled(fit.increasing_rad), + circle=circle, + maximum_monotonic_correction_rad=( + float(fit.maximum_monotonic_correction_rad) * scale + ), + maximum_hysteresis_rad=float(fit.maximum_hysteresis_rad) * scale, + quality=quality, + zero_offset_rad=float(fit.zero_offset_rad) * scale, + ) + return result + + +def derive_right_19_mechanical_endpoint_offsets( source_urdf: str | Path, curves: Mapping[str, JointCurveFit], *, maximum_offset_rad: float = math.radians(5.0), ) -> dict[str, float]: - """Estimate flexion origins from the measured full-flexion contact. + """Estimate encoder origins from repeatable measured mechanical endpoints. - G20 feedback 0 is the independently verified palm-contact endpoint. The - source CAD upper angle describes that same physical endpoint. Therefore - ``origin_offset + measured_endpoint == CAD_upper``. This makes every - returned value session-derived while avoiding the fixed planar-PnP phase - bias that affected the parallel MCP/PIP/DIP axes. + For the four fingers feedback 0 is the independently verified palm-contact + endpoint; thumb MCP uses its independently verified actuator endpoint. + The source URDF upper limit describes those same physical endpoints. + Therefore ``origin_offset + measured_travel == CAD_upper``. + + The coupled thumb-CMC axes are deliberately excluded. Physical + inspection of G20_RIGHT_001 proved that their electrical actuator + endpoints do not equal the source-CAD upper coordinates. Treating them + as equal produced repeatable but physically wrong zero offsets. + + The returned corrections are measured, generally non-zero encoder zeros; + they do not retain the CAD origin. Using full relative rotation travel + makes them invariant to rigid hand movement and arbitrary fixed Tag + mounting rotation, unlike a cross-camera absolute planar-PnP phase. """ if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: - raise ValueError("flexion endpoint maximum offset must be positive") + raise ValueError("mechanical endpoint maximum offset must be positive") root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() joints = {str(node.get("name")): node for node in root.findall("joint")} result: dict[str, float] = {} - for name in sorted(RIGHT_19_FLEXION_ENDPOINT_JOINTS): + for name in sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS): fit = curves.get(name) joint = joints.get(name) limit = None if joint is None else joint.find("limit") @@ -279,7 +424,18 @@ def derive_right_19_flexion_endpoint_offsets( if limit is None or limit.get("upper") is None: raise ValueError(f"source URDF joint {name} has no upper limit") upper = float(limit.get("upper")) - measured_endpoint = float(np.max(np.asarray(fit.angle_rad, dtype=float))) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + measured_endpoint = float(curve[0]) + if measured_endpoint <= 0.0 or abs(float(curve[255])) > math.radians(0.05): + raise ValueError( + f"{name} endpoint curve does not use command 255 as its zero reference" + ) + if abs(measured_endpoint - float(np.max(curve))) > math.radians(0.05): + raise ValueError( + f"{name} command 0 is not the measured upper mechanical endpoint" + ) offset = upper - measured_endpoint if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: raise ValueError( @@ -319,6 +475,47 @@ def circle_direction_is_constrained( name = str(joint_name) return name in constrained_circle_joints or name.endswith("_side") + +def select_cross_view_roll_direction_source( + primary_cone_residuals_rad: Sequence[float], + secondary_cone_residuals_rad: Sequence[float], + maximum_cone_mismatch_rad: float, +) -> str: + """Choose one roll-axis view for the complete repeated-sweep group. + + The choice must not be made independently for every cycle. A residual + sitting just either side of the cone gate can otherwise alternate the + selected camera and turn a fixed cross-view bias into a false cycle-axis + spread. The secondary view wins only with a three-quarter consensus and + a median residual inside the unchanged geometry gate; ambiguous groups + retain the primary view. + """ + primary = np.asarray(primary_cone_residuals_rad, dtype=float) + secondary = np.asarray(secondary_cone_residuals_rad, dtype=float) + if ( + primary.ndim != 1 + or secondary.ndim != 1 + or len(primary) != len(secondary) + or len(primary) < 3 + or not np.all(np.isfinite(primary)) + or not np.all(np.isfinite(secondary)) + or float(maximum_cone_mismatch_rad) <= 0.0 + ): + return "primary" + limit = float(maximum_cone_mismatch_rad) + required_consensus = int(math.ceil(0.75 * len(primary))) + primary_passes = int(np.count_nonzero(primary <= limit)) + secondary_passes = int(np.count_nonzero(secondary <= limit)) + if ( + secondary_passes >= required_consensus + and primary_passes < required_consensus + and float(np.median(secondary)) <= limit + and float(np.median(primary)) > limit + ): + return "secondary" + return "primary" + + ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 # Fit an axis-line point from the complete relative SE(3) trajectory instead @@ -833,6 +1030,555 @@ class JointAxisMeasurement: circle_axis_observability: float = 0.0 axis_point_source: str = "circle_center" pose_axis_line_rms_m: float = 0.0 + # Measurement record(s) that supplied pose_axis_line_rms_m. A combined + # cross-view axis may keep the front direction but take its physical line + # point and line-quality residual from the side alias. Retry logic must + # clear the actual quality source instead of blindly rescanning ``joint``. + pose_axis_line_source_joints: tuple[str, ...] = () + # Camera centre that observed ``point_common_xyz_m``. When populated, + # the zero solver can use only the ray from this centre to the fitted axis + # point. That ray is the depth-free interpretation-plane observation of + # the physical axis; translating a monocular planar-PnP solution along + # its optical ray therefore cannot rotate the recovered palm frame. + axis_point_camera_center_common_xyz_m: ( + tuple[float, float, float] | None + ) = None + # Normal of the source camera's interpretation plane for this axis line. + # Unlike a 3-D PnP line point, the plane is unchanged by optical-depth + # error. Several named parallel root lines jointly recover their common + # physical direction as the null direction of these plane normals. + axis_point_interpretation_plane_normal_common_xyz: ( + tuple[float, float, float] | None + ) = None + + +@dataclass(frozen=True) +class PalmOrientationMeasurement: + """Direction-only joint observation from a partially visible sweep. + + The moving Tag may disappear before the motor reaches its far endpoint. + Only the relative SO(3) trajectory is retained, so fixed Tag translation + and mounting rotation cannot define the palm phase. + """ + + source_joint: str + model_joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + observed_arc_rad: float + rotation_orthogonal_rms_rad: float + + +def fit_partial_palm_orientation_measurement( + source_joint: str, + model_joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), +) -> PalmOrientationMeasurement: + """Fit a physical axis direction without requiring full command travel.""" + samples = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(cycle) + ] + if len(samples) < 12: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible samples" + ) + if not 0.0 < float(minimum_arc_rad) < math.pi: + raise ValueError("palm orientation minimum arc must be in (0, pi)") + if float(maximum_rotation_orthogonal_rms_rad) <= 0.0: + raise ValueError( + "palm orientation rotation residual limit must be positive" + ) + reference = Rotation.from_quat( + _baseline_reference(samples, int(zero_command_u8)) + ) + samples_by_bin: dict[tuple[str, int], list[Mapping[str, Any]]] = {} + for record in samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + samples_by_bin.setdefault(key, []).append(record) + if len(samples_by_bin) < 6: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible command bins" + ) + binned_rotations = [ + Rotation.from_quat( + robust_rotation_summary( + [_relative_rotation(record) for record in group] + )[0] + ) + for group in samples_by_bin.values() + ] + vectors = [ + (reference.inv() * rotation).as_rotvec() + for rotation in binned_rotations + ] + axis_child = fit_rotation_axis( + vectors, [command for _direction, command in samples_by_bin] + ) + angles = np.asarray( + [float(vector @ axis_child) for vector in vectors], dtype=float + ) + observed_arc = float(np.ptp(angles)) + orthogonal_rms = float( + np.sqrt( + np.mean( + [ + np.linalg.norm( + vector - float(vector @ axis_child) * axis_child + ) + ** 2 + for vector in vectors + ] + ) + ) + ) + if observed_arc < float(minimum_arc_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} visible rotation arc " + f"{math.degrees(observed_arc):.3f}deg is below " + f"{math.degrees(minimum_arc_rad):.3f}deg" + ) + if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} rotation residual " + f"{math.degrees(orthogonal_rms):.3f}deg exceeds " + f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg" + ) + zero_records = _near_zero_records(samples, int(zero_command_u8)) + if not zero_records: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has no visible zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + axis_parent = reference.apply(axis_child) + axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) + axis_common /= np.linalg.norm(axis_common) + state = np.median( + np.asarray( + [record["state_u8"] for record in zero_records], dtype=float + ), + axis=0, + ) + return PalmOrientationMeasurement( + source_joint=str(source_joint), + model_joint=str(model_joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + condition_state_u8=tuple(float(value) for value in state), + observed_arc_rad=observed_arc, + rotation_orthogonal_rms_rad=orthogonal_rms, + ) + + +def fit_partial_palm_orientation_measurements( + *, + sources: Mapping[str, str], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + motor_by_source: Mapping[str, int], + baseline_command_u8: Sequence[int], + cycles: Sequence[int], + minimum_sources: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), +) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]: + """Fit every usable optional source and require redundant coverage.""" + source_map = {str(name): str(model) for name, model in sources.items()} + minimum = int(minimum_sources) + if not source_map: + if minimum != 0: + raise ValueError( + "palm orientation minimum is non-zero without sources" + ) + return (), {} + if minimum < 2 or minimum > len(source_map): + raise ValueError( + "palm orientation minimum source count must be between two " + "and the configured source count" + ) + missing_motors = sorted(set(source_map) - set(motor_by_source)) + if missing_motors: + raise ValueError( + "palm orientation sources are missing motor mappings: " + + ",".join(missing_motors) + ) + fitted: list[PalmOrientationMeasurement] = [] + rejected: dict[str, str] = {} + for cycle in (int(value) for value in cycles): + cycle_fitted: list[PalmOrientationMeasurement] = [] + for source_joint, model_joint in source_map.items(): + motor = int(motor_by_source[source_joint]) + try: + measurement = fit_partial_palm_orientation_measurement( + source_joint, + model_joint, + records_by_joint.get(source_joint, ()), + cycle=cycle, + zero_command_u8=int(baseline_command_u8[motor]), + minimum_arc_rad=minimum_arc_rad, + maximum_rotation_orthogonal_rms_rad=( + maximum_rotation_orthogonal_rms_rad + ), + ) + except Exception as error: + rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error) + continue + cycle_fitted.append(measurement) + if len(cycle_fitted) < minimum: + cycle_reasons = { + key: value + for key, value in rejected.items() + if key.endswith(f":cycle{cycle + 1}") + } + raise ValueError( + f"palm orientation cycle {cycle + 1} has " + f"{len(cycle_fitted)}/{minimum} usable sources: " + + "; ".join( + f"{key}={value}" for key, value in cycle_reasons.items() + ) + ) + fitted.extend(cycle_fitted) + return tuple(fitted), rejected + + +def with_depth_free_axis_projection( + measurement: JointAxisMeasurement, + camera_center_common_xyz_m: Sequence[float], +) -> JointAxisMeasurement: + """Attach the source-camera interpretation plane of an axis line. + + The plane is defined by the camera centre and the fitted 3-D line, but is + only a projective observation: moving either fitted line point along its + optical ray leaves the plane unchanged. A fixed Tag mount changes the + moving point trajectory, not its recovered physical screw axis. + """ + camera_center = _vector( + camera_center_common_xyz_m, 3, name="axis camera centre" + ) + point = np.asarray(measurement.point_common_xyz_m, dtype=float) + axis = np.asarray(measurement.axis_common_xyz, dtype=float) + axis /= np.linalg.norm(axis) + ray = point - camera_center + ray_norm = float(np.linalg.norm(ray)) + if ray_norm <= 1.0e-6: + raise ValueError("axis point coincides with its source camera") + plane_normal = np.cross(ray / ray_norm, axis) + plane_norm = float(np.linalg.norm(plane_normal)) + if plane_norm <= 1.0e-6: + raise ValueError("axis projection is degenerate in its source camera") + plane_normal /= plane_norm + return replace( + measurement, + axis_point_camera_center_common_xyz_m=tuple( + float(value) for value in camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) for value in plane_normal + ), + ) + + +def cross_view_side_line_source( + measurement: JointAxisMeasurement, +) -> str | None: + """Return the side alias that supplied a fallback axis-line point.""" + sources = tuple( + str(source) + for source in getattr( + measurement, "pose_axis_line_source_joints", () + ) + ) + if ( + getattr(measurement, "axis_point_source", "") + in { + "side_circle_cross_view", + "side_circle_shared_radius_cross_view", + "side_interpretation_plane_cross_view", + } + and len(sources) == 1 + and sources[0].endswith("_side") + ): + return sources[0] + return None + + +def axis_line_uses_depth_free_interpretation_plane( + measurement: JointAxisMeasurement, +) -> bool: + """Return whether only the source-camera bearing is geometrically used.""" + return bool( + measurement.axis_point_source + in { + "side_interpretation_plane_cross_view", + "front_interpretation_plane_cross_view_validated", + } + and measurement.axis_point_camera_center_common_xyz_m is not None + ) + + +def refit_axis_line_group_with_shared_radius( + measurements: Sequence[JointAxisMeasurement], + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + canonical_zero_direction: str | None, +) -> tuple[JointAxisMeasurement, ...]: + """Refit repeated cross-view axis lines with one physical radius. + + A side-view roll alias observes the same child Tag and the same physical + lever arm in every cycle. Fitting an independent radius to each short, + near-edge-on arc leaves radius and circle centre strongly correlated; + sub-pixel PnP noise can then move the reported axis line by several + millimetres even though every trajectory has a low radial residual. + + Keep a separate centre (and therefore an independent line-repeatability + check) for every cycle, but solve one shared radius from all cycles. This + is a physical constraint rather than a relaxed quality gate: the returned + lines are still checked against the unchanged cycle RMS limit, and each + cycle's radial residual remains an independent hard check. + """ + group = tuple(measurements) + if len(group) < 2: + return group + cycles = [int(measurement.cycle) for measurement in group] + if len(set(cycles)) != len(cycles): + raise ValueError("shared-radius axis group contains duplicate cycles") + + entries: list[dict[str, Any]] = [] + initial_parameters: list[float] = [] + initial_radii: list[float] = [] + for measurement in group: + cycle_records = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(measurement.cycle) + ] + motion_records = [ + record + for record in cycle_records + if str(record.get("kind", "sample")) != "baseline_hold_sample" + ] + if len(motion_records) < 6: + motion_records = cycle_records + if len(motion_records) < 6: + raise ValueError( + f"cycle {measurement.cycle + 1} has too few shared-radius samples" + ) + + reference_records = _canonical_reference_records( + cycle_records, canonical_zero_direction + ) + zero_records = _near_zero_records( + reference_records, int(zero_command_u8) + ) + if not zero_records: + raise ValueError( + f"cycle {measurement.cycle + 1} has no shared-radius zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + common_axis = _vector( + measurement.axis_common_xyz, 3, name="shared-radius common axis" + ) + common_axis /= np.linalg.norm(common_axis) + parent_axis = parent_rotation.inv().apply(common_axis) + parent_axis /= np.linalg.norm(parent_axis) + + points = np.asarray( + [record["relative_translation_xyz_m"] for record in motion_records], + dtype=float, + ) + basis_x, basis_y = _plane_basis(parent_axis) + origin = np.mean(points, axis=0) + local = points - origin + points_xy = np.column_stack((local @ basis_x, local @ basis_y)) + initial = _fit_circle_with_axis(points, parent_axis) + initial_center = _vector( + initial["center_xyz_m"], 3, name="initial shared-radius centre" + ) + initial_parameters.extend( + [ + float((initial_center - origin) @ basis_x), + float((initial_center - origin) @ basis_y), + ] + ) + initial_radii.append(float(initial["radius_m"])) + entries.append( + { + "measurement": measurement, + "points": points, + "points_xy": points_xy, + "origin": origin, + "basis_x": basis_x, + "basis_y": basis_y, + "parent_axis": parent_axis, + "parent_rotation": parent_rotation, + "parent_translation": parent_translation, + } + ) + + median_radius = float(np.median(initial_radii)) + if median_radius <= 1.0e-6 or not math.isfinite(median_radius): + raise ValueError("shared-radius axis group has an invalid initial radius") + initial_value = np.asarray( + [*initial_parameters, median_radius], dtype=float + ) + + def residual(parameters: np.ndarray) -> np.ndarray: + radius = float(parameters[-1]) + return np.concatenate( + [ + np.linalg.norm( + entry["points_xy"] + - parameters[2 * index : 2 * index + 2], + axis=1, + ) + - radius + for index, entry in enumerate(entries) + ] + ) / 0.0005 + + lower = np.full(initial_value.shape, -np.inf, dtype=float) + upper = np.full(initial_value.shape, np.inf, dtype=float) + lower[-1] = max(1.0e-6, 0.5 * median_radius) + upper[-1] = 2.0 * median_radius + solution = least_squares( + residual, + initial_value, + bounds=(lower, upper), + loss="soft_l1", + f_scale=1.0, + max_nfev=3000, + ) + if not solution.success: + raise ValueError( + "shared-radius axis optimization failed: " + solution.message + ) + + shared_radius = float(solution.x[-1]) + if not math.isfinite(shared_radius): + raise ValueError("shared-radius axis optimization is non-finite") + result: list[JointAxisMeasurement] = [] + for index, entry in enumerate(entries): + center_xy = solution.x[2 * index : 2 * index + 2] + center_parent = ( + entry["origin"] + + float(center_xy[0]) * entry["basis_x"] + + float(center_xy[1]) * entry["basis_y"] + ) + # The coordinate along an infinite axis is a gauge. Retain the + # robust centre of this cycle's observed axial coordinates. + axial_offsets = ( + entry["points"] - center_parent + ) @ entry["parent_axis"] + center_parent += float(np.median(axial_offsets)) * entry["parent_axis"] + center_common = ( + entry["parent_rotation"].apply(center_parent) + + entry["parent_translation"] + ) + radial_residual = ( + np.linalg.norm( + entry["points_xy"] - center_xy, + axis=1, + ) + - shared_radius + ) + radial_rms = float(np.sqrt(np.mean(np.square(radial_residual)))) + measurement = entry["measurement"] + result.append( + replace( + measurement, + point_common_xyz_m=tuple( + float(value) for value in center_common + ), + radial_rms_m=max(float(measurement.radial_rms_m), radial_rms), + axis_point_source="side_circle_shared_radius_cross_view", + ) + ) + return tuple(result) + + +def maximum_axis_line_cycle_spread_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Measure repeatability of independently fitted near-parallel lines. + + Axis-line points have an arbitrary coordinate along their own direction. + Compare only the perpendicular displacement, symmetrically against both + fitted directions, so that the result remains meaningful with the small + allowed cycle-to-cycle direction variation. + """ + maximum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + maximum = max( + maximum, + float(np.linalg.norm(np.cross(delta, left_axis))), + float(np.linalg.norm(np.cross(delta, right_axis))), + ) + return maximum + + +def axis_line_cycle_rms_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Return RMS line-position scatter about the four-cycle consensus. + + For parallel lines, the sum of squared pairwise distances divided by + ``n**2`` equals the mean squared distance from their centroid. Averaging + each pair's distance against both near-parallel directions preserves that + identity while avoiding an arbitrary choice of one cycle's direction. + """ + count = len(measurements) + if count < 2: + return 0.0 + squared_pairwise_sum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + left_distance = float(np.linalg.norm(np.cross(delta, left_axis))) + right_distance = float(np.linalg.norm(np.cross(delta, right_axis))) + squared_pairwise_sum += 0.5 * ( + left_distance**2 + right_distance**2 + ) + return float(math.sqrt(squared_pairwise_sum / (count**2))) def _fit_axis_point_from_pose_trajectory( @@ -1223,6 +1969,7 @@ def fit_joint_axis_measurement( circle_axis_observability=float(circle_axis_observability), axis_point_source=axis_point_source, pose_axis_line_rms_m=pose_axis_line_rms_m, + pose_axis_line_source_joints=(str(joint),), ) @@ -1405,6 +2152,8 @@ class ZeroSolveResult: observability_parameter_count: int observability_condition_number: float offset_covariance_rad2: Mapping[str, float] + axis_cone_mismatch_by_joint_rad: Mapping[str, float] + axis_cone_bias_classification_by_joint: Mapping[str, str] failure_reasons: Mapping[str, str] @@ -1430,6 +2179,9 @@ def solve_urdf_zero_offsets( *, source_urdf: str | Path, measurements: Sequence[JointAxisMeasurement], + palm_orientation_measurements: Sequence[ + PalmOrientationMeasurement + ] = (), curves: Mapping[str, JointCurveFit], motor_by_joint: Mapping[str, int], training_cycles: Sequence[int] = (0, 1), @@ -1445,6 +2197,7 @@ def solve_urdf_zero_offsets( maximum_validation_error_rad: float | None = None, maximum_confidence_half_width_rad: float | None = None, maximum_axis_cone_mismatch_rad: float = math.radians(5.0), + maximum_systematic_axis_cone_bias_rad: float | None = None, maximum_pose_axis_line_rms_m: float = 0.001, maximum_observability_condition_number: float = 1.0e10, hand_type: str = "left", @@ -1454,12 +2207,12 @@ def solve_urdf_zero_offsets( ) -> ZeroSolveResult: """Solve all directly observable zero offsets without fitting CAD errors. - The palm pose uses the most observable root-axis direction together with - the separation of the two root lines. Encoder zeros are then fitted from - one angular residual per joint: downstream-axis direction for non-parallel - pairs and the radial phase between parallel axis lines. This prevents link - length, along-axis Tag placement, and monocular depth bias from being - absorbed as an encoder-zero correction. + The product palm pose uses the robust common root-axis direction together + with independent partial-sweep MCP-pitch directions; root-line points + determine translation only. Legacy layouts retain their single observed + reference chain. Encoder zeros are then fitted from one angular residual + per joint. This prevents link length, along-axis Tag placement and + monocular depth bias from becoming encoder-zero corrections. """ profile = get_zero_calibration_profile(hand_type, tag_layout) model = UrdfKinematicModel(source_urdf) @@ -1471,6 +2224,62 @@ def solve_urdf_zero_offsets( or {m.joint for m in validation} != expected ): raise ValueError("axis measurements do not contain all required joints/cycles") + configured_palm_sources = { + str(name): str(model_joint) + for name, model_joint in ( + profile.hand.palm_orientation_sources or {} + ).items() + } + orientation_measurements = tuple(palm_orientation_measurements) + if configured_palm_sources: + minimum_orientation_sources = int( + profile.hand.minimum_palm_orientation_sources + ) + unknown_orientation_sources = sorted( + { + item.source_joint + for item in orientation_measurements + if item.source_joint not in configured_palm_sources + or configured_palm_sources[item.source_joint] + != item.model_joint + } + ) + if unknown_orientation_sources: + raise ValueError( + "palm orientation measurements do not match the product " + "profile: " + + ",".join(unknown_orientation_sources) + ) + required_orientation_cycles = { + *(int(value) for value in training_cycles), + int(validation_cycle), + } + for cycle in required_orientation_cycles: + cycle_sources = { + item.source_joint + for item in orientation_measurements + if int(item.cycle) == cycle + } + if len(cycle_sources) < minimum_orientation_sources: + raise ValueError( + f"palm orientation cycle {cycle + 1} contains " + f"{len(cycle_sources)}/{minimum_orientation_sources} " + "qualified sources" + ) + elif orientation_measurements: + raise ValueError( + "palm orientation measurements were supplied without a profile" + ) + orientation_training = tuple( + item + for item in orientation_measurements + if int(item.cycle) in set(int(value) for value in training_cycles) + ) + orientation_validation = tuple( + item + for item in orientation_measurements + if int(item.cycle) == int(validation_cycle) + ) if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: raise ValueError("finger maximum offset must be positive and no larger than thumb") joint_limits = { @@ -1494,6 +2303,18 @@ def solve_urdf_zero_offsets( raise ValueError("zero significance thresholds must be non-negative") if maximum_axis_cone_mismatch_rad <= 0.0: raise ValueError("maximum axis cone mismatch must be positive") + if ( + maximum_systematic_axis_cone_bias_rad is not None + and ( + maximum_systematic_axis_cone_bias_rad + <= maximum_axis_cone_mismatch_rad + or maximum_systematic_axis_cone_bias_rad > math.radians(90.0) + ) + ): + raise ValueError( + "maximum systematic axis cone bias must exceed the precision " + "limit and be at most 90deg" + ) if maximum_pose_axis_line_rms_m <= 0.0: raise ValueError("maximum pose axis-line RMS must be positive") if maximum_observability_condition_number <= 1.0: @@ -1530,6 +2351,23 @@ def solve_urdf_zero_offsets( zero_offsets=offsets, joint_angles=angles, ) + + def predicted_palm_orientation_local( + measurement: PalmOrientationMeasurement, + offsets: Mapping[str, float], + ) -> np.ndarray: + angles = _angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + axis, _ = model.axis_line( + measurement.model_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + return axis / np.linalg.norm(axis) fixed_offsets = { str(name): float(value) for name, value in ( @@ -1600,30 +2438,7 @@ def solve_urdf_zero_offsets( raise ValueError("root axis average is degenerate") return result / norm - # The two CAD root axes are parallel, so their directions alone cannot - # resolve palm rotation about that common direction. Do not use the - # 3-D separation of two monocularly fitted axis lines for this purpose: - # a fixed optical-depth error rotates the fitted palm frame and appears - # as an equally stable thumb-roll zero bias. Four-finger static splay - # is deliberately fixed to the source-CAD straight pose, so the - # measured reference-finger MCP-pitch direction is the independent, - # non-parallel orientation anchor. Root lines remain translation-only - # anchors. root_names = tuple(sorted(profile.root_anchor_joints)) - primary = max( - root_names, - key=lambda name: float(np.ptp(np.asarray(curves[name].angle_rad))), - ) - secondary = next(name for name in root_names if name != primary) - orientation_anchor = f"{profile.reference_finger}_mcp_pitch" - orientation_items = [ - item for item in selected if item.joint == orientation_anchor - ] - if not orientation_items: - raise ValueError( - f"zero solve is missing palm orientation anchor {orientation_anchor}" - ) - predicted_axes: dict[str, np.ndarray] = {} predicted_points: dict[str, np.ndarray] = {} observed_axes: dict[str, np.ndarray] = {} @@ -1921,6 +2736,24 @@ def solve_urdf_zero_offsets( ) return rotation, translation + # Legacy layouts have only one observed finger chain. Their + # non-parallel reference-finger pitch axis remains the orientation + # anchor; the product profile above deliberately does not use it. + primary = max( + root_names, + key=lambda name: float( + np.ptp(np.asarray(curves[name].angle_rad)) + ), + ) + orientation_anchor = f"{profile.reference_finger}_mcp_pitch" + orientation_items = [ + item for item in selected if item.joint == orientation_anchor + ] + if not orientation_items: + raise ValueError( + f"zero solve is missing palm orientation anchor " + f"{orientation_anchor}" + ) predicted_orientation_axis = undirected_axis_average( [predicted_local(item, zero_offsets)[0] for item in orientation_items] ) @@ -2164,6 +2997,11 @@ def solve_urdf_zero_offsets( ).items() } + product_finger_rolls = tuple( + name + for name in profile.direct_zero_joints + if name.endswith("_mcp_roll") and not name.startswith("thumb_") + ) offset_limits = np.asarray( [ joint_limits.get( @@ -2187,6 +3025,21 @@ def solve_urdf_zero_offsets( math.radians(90.0), np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + # The absolute rotation of the fitted palm frame about the four + # parallel roll axes is a shared gauge. Each physical finger zero is + # only its deviation from the four-finger median, but the raw scalar + # solves include that common mode. Give those four diagnostic solves + # enough range for the unchanged global common-mode limit plus the + # unchanged per-finger deviation limit; applying the per-finger bound + # before removing the gauge clips every solve to the same value and + # destroys the observable deviations. + for index, name in enumerate(profile.direct_zero_joints): + if name in product_finger_rolls: + diagnostic_offset_limits[index] = min( + math.radians(90.0), + maximum_offset_rad + offset_limits[index], + ) def optimise_offsets( selected: Sequence[JointAxisMeasurement], @@ -2360,6 +3213,9 @@ def solve_urdf_zero_offsets( for index, name in enumerate(profile.direct_zero_joints) } + axis_cone_mismatch_by_joint: dict[str, float] = {} + axis_cone_bias_classification_by_joint: dict[str, str] = {} + def zero_observation_failure_reasons( selected: Sequence[JointAxisMeasurement], offsets: Mapping[str, float], @@ -2376,7 +3232,7 @@ def solve_urdf_zero_offsets( ] if observer_joint in profile.axis_parent_joint: parent_joint = profile.axis_parent_joint[observer_joint] - maximum_cone_mismatch = 0.0 + cone_mismatches: list[float] = [] for item in observer_items: state = ( item.condition_state_u8 @@ -2401,18 +3257,43 @@ def solve_urdf_zero_offsets( zero_offsets=offsets, joint_angles=angles, ) - maximum_cone_mismatch = max( - maximum_cone_mismatch, + cone_mismatches.append( _axis_cone_mismatch_rad( rotation.apply(predicted_axis), item.axis_common_xyz, rotation.apply(parent_axis), - ), + ) ) + maximum_cone_mismatch = max(cone_mismatches) + axis_cone_mismatch_by_joint[offset_joint] = ( + maximum_cone_mismatch + ) if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: - failures[offset_joint] = ( - "zero_axis_cone_mismatch_too_large" + cone_range = float(np.ptp(cone_mismatches)) + stable_product_bias = bool( + profile.hand.supports("stable_cross_view_cone_bias") + and maximum_systematic_axis_cone_bias_rad is not None + and len(cone_mismatches) >= 4 + and maximum_cone_mismatch + <= maximum_systematic_axis_cone_bias_rad + and cone_range <= maximum_cycle_difference_rad ) + if stable_product_bias: + # A parent zero rotates the downstream direction + # around the parent axis and cannot change their cone + # angle. The zero-sensitive residual above already + # projects both directions onto the parent-normal + # plane, so a repeatable cross-camera/planar-PnP cone + # bias cannot corrupt the written encoder zero. Keep + # it visible in the result while retaining the gross + # gate for a wrong axis, loose Tag, or moved camera. + axis_cone_bias_classification_by_joint[offset_joint] = ( + "stable_cross_view_or_planar_pnp_bias" + ) + else: + failures[offset_joint] = ( + "zero_axis_cone_mismatch_too_large" + ) else: parent_joint = profile.phase_parent_joint[observer_joint] phase_items: list[JointAxisMeasurement] = [] @@ -2551,6 +3432,38 @@ def solve_urdf_zero_offsets( improvement_by_joint: dict[str, float] = {} improvement_confidence_lower: dict[str, float] = {} improvement_passed = True + palm_orientation_validation_errors = tuple( + math.acos( + abs( + float( + np.clip( + base_rotation.apply( + predicted_palm_orientation_local( + item, zero_offsets + ) + ) + @ np.asarray(item.axis_common_xyz, dtype=float), + -1.0, + 1.0, + ) + ) + ) + ) + for item in orientation_validation + ) + palm_orientation_validation_limit = ( + maximum_validation_error_rad + if maximum_validation_error_rad is not None + else maximum_axis_cone_mismatch_rad + ) + palm_orientation_validation_passed = bool( + not configured_palm_sources + or ( + palm_orientation_validation_errors + and float(np.median(palm_orientation_validation_errors)) + <= palm_orientation_validation_limit + ) + ) for offset_joint, observer_joint in profile.offset_observer_joint.items(): if offset_joint in fixed_offsets: improvement_by_joint[offset_joint] = 0.0 @@ -2664,11 +3577,6 @@ def solve_urdf_zero_offsets( np.sqrt(np.mean(np.square(all_validation_line_errors))) ) ) - product_finger_rolls = tuple( - name - for name in profile.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) finger_roll_common_mode = ( float( np.median( @@ -2716,6 +3624,10 @@ def solve_urdf_zero_offsets( if abs(checked_offset) >= limit - math.radians(0.01): diagnostic_bound_hits.append(name) failure_reasons: dict[str, str] = {} + if not palm_orientation_validation_passed: + failure_reasons["palm_orientation"] = ( + "palm_orientation_holdout_too_large" + ) if ( profile.hand.layout_id == G20_RIGHT_19_LAYOUT and observability_rank < observability_parameter_count @@ -2788,6 +3700,7 @@ def solve_urdf_zero_offsets( for reason in failure_reasons.values() ) and improvement_passed + and palm_orientation_validation_passed ) # Never refit a model that has passed its holdout with the validation @@ -2848,6 +3761,10 @@ def solve_urdf_zero_offsets( observability_parameter_count=observability_parameter_count, observability_condition_number=observability_condition_number, offset_covariance_rad2=offset_covariance, + axis_cone_mismatch_by_joint_rad=axis_cone_mismatch_by_joint, + axis_cone_bias_classification_by_joint=( + axis_cone_bias_classification_by_joint + ), failure_reasons=failure_reasons, ) diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py index 48b5ca1..e0b0661 100644 --- a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py @@ -39,16 +39,21 @@ def _default_source_urdf(hand_type: str) -> Path: def _launch_stack(context): + from g20_thumb_apriltag_calibration.product import ( + get_product_calibration_contract, + ) + + model = LaunchConfiguration("model").perform(context).strip().upper() hand_type = LaunchConfiguration("hand_type").perform(context).lower() if hand_type not in {"left", "right"}: raise RuntimeError("hand_type must be left or right") tag_layout = LaunchConfiguration("tag_layout").perform(context).lower() - if tag_layout not in {"legacy_11", "g20_right_15", "g20_right_19"}: - raise RuntimeError( - "tag_layout must be legacy_11, g20_right_15 or g20_right_19" + try: + contract = get_product_calibration_contract( + model, hand_type, tag_layout ) - if tag_layout in {"g20_right_15", "g20_right_19"} and hand_type != "right": - raise RuntimeError("G20 right product layout requires hand_type:=right") + except ValueError as error: + raise RuntimeError(str(error)) from error requested_tag_config = LaunchConfiguration("tag_config").perform(context) package_share = Path( get_package_share_directory("g20_thumb_apriltag_calibration") @@ -68,9 +73,10 @@ def _launch_stack(context): ) if not tag_config.is_file(): raise RuntimeError(f"tag config does not exist: {tag_config}") - 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" + topic_prefix = f"/{model.lower()}" + command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd" + state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state" + info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info" requested_source = LaunchConfiguration("source_urdf_path").perform(context) source_urdf = ( Path(requested_source).expanduser().resolve() @@ -82,7 +88,7 @@ def _launch_stack(context): expected_source_hash = LaunchConfiguration( "source_urdf_expected_sha256" ).perform(context).strip().lower() - if tag_layout in {"g20_right_15", "g20_right_19"}: + if contract.profile.supports("urdf_zero_publication"): if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: raise RuntimeError( "G20 right product layout requires source_urdf_expected_sha256 confirmed " @@ -239,10 +245,10 @@ def _launch_stack(context): parameters=[ { "hand_type": hand_type, - "hand_joint": "G20", + "hand_joint": model, "can": LaunchConfiguration("can_interface"), "modbus": "None", - "topic_prefix": "/g20", + "topic_prefix": topic_prefix, "move_on_startup": False, "startup_speed": ParameterValue( LaunchConfiguration("calibration_speed"), value_type=int @@ -274,6 +280,7 @@ def _launch_stack(context): LaunchConfiguration("calibration_config"), { "serial_number": hand_serial, + "model": model, "hand_type": hand_type, "tag_layout": tag_layout, "session_dir": str(session_dir), @@ -396,6 +403,7 @@ def generate_launch_description() -> LaunchDescription: name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), + DeclareLaunchArgument("model", default_value="G20"), DeclareLaunchArgument("hand_type", default_value="left"), DeclareLaunchArgument("tag_layout", default_value="legacy_11"), DeclareLaunchArgument("serial_number", default_value="UNSET"), diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index ca6c069..c5a1fb1 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -26,7 +26,7 @@ setup( zip_safe=True, maintainer="lxp", maintainer_email="support@linker-robotics.com", - description="One-command three-camera AprilTag calibration for the G20 right hand", + description="Profile-driven three-camera AprilTag hand calibration", license="MIT", entry_points={ "console_scripts": [ @@ -68,6 +68,10 @@ setup( "calibrate_g20_right = " "g20_thumb_apriltag_calibration.one_command:main" ), + ( + "calibrate_hand = " + "g20_thumb_apriltag_calibration.one_command:main" + ), ], }, ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index 1d28d68..f4fcd9a 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -206,7 +206,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["zero_maximum_observability_condition_number"] >= 1.0 assert parameters["zero_maximum_offset_deg"] <= 20.0 assert parameters["zero_finger_maximum_offset_deg"] <= 3.0 - assert parameters["flexion_endpoint_maximum_offset_deg"] == 5.0 + assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py index 6762364..e06b927 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -28,6 +28,7 @@ from g20_thumb_apriltag_calibration.full_hand import ( clamp_runtime_fits_to_urdf_limits, center_splay_curve, compare_cross_view_roll_curves, + cross_view_roll_diagnostic_metrics, derive_mimic_passive_fits, fit_joint_center_curve, fit_joint_image_curve, @@ -98,6 +99,8 @@ def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None: "pinky_mcp_roll", "pinky_mcp_roll_side", ) + assert profile.palm_axis_observers == () + assert profile.minimum_palm_orientation_sources == 0 assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip") assert { name: spec.source_joint @@ -411,6 +414,98 @@ def test_cross_view_roll_curve_must_agree_before_fusion() -> None: compare_cross_view_roll_curves(fit, reverse) +def test_cross_view_roll_curve_allows_bounded_projection_scale() -> None: + values = tuple(0.5 * (127 - command) / 127 for command in range(256)) + primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + projected = tuple(0.8 * value for value in values) + secondary = replace( + primary, + angle_rad=projected, + decreasing_rad=projected, + increasing_rad=projected, + ) + + with pytest.raises(ValueError, match="cross_view_roll_curve"): + compare_cross_view_roll_curves(primary, secondary) + + metrics = compare_cross_view_roll_curves( + primary, + secondary, + allow_projection_scale=True, + ) + + assert metrics["projection_scale"] == pytest.approx(1.25) + assert metrics["angle_rad_rms_difference_rad"] == pytest.approx(0.0) + assert metrics["raw_angle_rad_rms_difference_rad"] > 0.0 + + +def test_projected_cross_view_keeps_branch_gap_as_diagnostic() -> None: + values = tuple(0.5 * (127 - command) / 127 for command in range(256)) + primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + shifted_increasing = tuple( + value + math.radians(0.5) for value in values + ) + secondary = replace(primary, increasing_rad=shifted_increasing) + + metrics = compare_cross_view_roll_curves( + primary, + secondary, + allow_projection_scale=True, + ) + + assert metrics["baseline_branch_gap_difference_rad"] > math.radians(0.3) + assert metrics["baseline_branch_gap_gate_applied"] == 0.0 + + +def test_cross_view_roll_curve_rejects_implausible_projection_scale() -> None: + values = tuple(0.5 * (127 - command) / 127 for command in range(256)) + primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + collapsed = tuple(0.2 * value for value in values) + secondary = replace( + primary, + angle_rad=collapsed, + decreasing_rad=collapsed, + increasing_rad=collapsed, + ) + + with pytest.raises(ValueError, match="projection_scale_ratio"): + compare_cross_view_roll_curves( + primary, + secondary, + allow_projection_scale=True, + ) + + +def test_validation_only_cross_view_metrics_never_create_a_retry_gate() -> None: + values = tuple(0.5 * (127 - command) / 127 for command in range(256)) + primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + biased = tuple( + value + + math.radians(2.5) * math.sin(math.pi * command / 255.0) + for command, value in enumerate(values) + ) + secondary = replace( + primary, + angle_rad=biased, + decreasing_rad=biased, + increasing_rad=biased, + ) + + metrics = cross_view_roll_diagnostic_metrics(primary, secondary) + + assert metrics["angle_rad_rms_difference_rad"] > math.radians(1.0) + + reversed_secondary = replace( + secondary, + angle_rad=tuple(-value for value in secondary.angle_rad), + decreasing_rad=tuple(-value for value in secondary.decreasing_rad), + increasing_rad=tuple(-value for value in secondary.increasing_rad), + ) + assert cross_view_roll_diagnostic_metrics( + primary, reversed_secondary + ) == {"direction_disagrees": 1.0} + + def _records() -> list[dict[str, object]]: commands = list(range(0, 256, 16)) if commands[-1] != 255: diff --git a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py index 88fc414..e1fe3dc 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py +++ b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py @@ -4,6 +4,7 @@ import json import math from pathlib import Path from types import SimpleNamespace +from xml.etree import ElementTree as ET import numpy as np import pytest @@ -39,6 +40,7 @@ from g20_thumb_apriltag_calibration.publication import ( atomic_session_pointer, build_mujoco_validation_commands, finalize_session_artifacts, + session_artifact_paths, validate_runtime_curves_against_urdf_limits, verify_corrected_urdf, verify_urdf_mesh_resources, @@ -52,6 +54,8 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( _combination_motor_directions, _combination_validation_items, _model_link_in_observer_base, + _palm_axis_observer_schema, + _palm_axis_resume_policy, _steady_checkpoint_commands, _unresolved_fit_failure_tasks, combination_target_coverage, @@ -59,7 +63,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( resumable_completed_task_prefix, ) from g20_thumb_apriltag_calibration.urdf_zero import ( - RIGHT_19_FLEXION_ENDPOINT_JOINTS, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, UrdfKinematicModel, get_zero_calibration_profile, write_zero_corrected_urdf, @@ -79,8 +83,15 @@ def _config(tmp_path: Path, *, passes: int = 2): ) -def _payload(serial: str = "G20_RIGHT_001") -> dict: +def _payload( + serial: str = "G20_RIGHT_001", + *, + zero_offsets: dict[str, float] | None = None, +) -> dict: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + offsets = zero_offsets or { + name: 0.01 for name in profile.active_joints + } baseline = [255] * 20 baseline[6:10] = [127] * 4 fits = {} @@ -100,7 +111,7 @@ def _payload(serial: str = "G20_RIGHT_001") -> dict: return build_compact_payload( serial_number=serial, measured_fits=fits, - urdf_zero_offsets_rad={name: 0.01 for name in profile.active_joints}, + urdf_zero_offsets_rad=offsets, validation_errors_rad=[0.0, math.radians(0.5)], passed=True, baseline=baseline, @@ -109,10 +120,18 @@ def _payload(serial: str = "G20_RIGHT_001") -> dict: ) -def _make_passed_session(config, stamp: str) -> Path: +def _make_passed_session( + config, + stamp: str, + *, + zero_offsets: dict[str, float] | None = None, +) -> Path: session = config.session_root / stamp session.mkdir(parents=True) - payload = _payload(config.serial_number) + offsets = zero_offsets or { + name: 0.01 for name in ACTIVE_ZERO_JOINTS + } + payload = _payload(config.serial_number, zero_offsets=offsets) atomic_write_json( session / f"g20_right_{config.serial_number}_calibration.json", payload ) @@ -120,9 +139,10 @@ def _make_passed_session(config, stamp: str) -> Path: source_urdf=config.source_urdf, output_directory=session, serial_number=config.serial_number, - offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS}, + offsets_rad=offsets, endpoint_anchored_offsets_rad={ - name: 0.01 for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS + name: offsets[name] + for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS }, timestamp=stamp, ) @@ -156,6 +176,18 @@ def _passed_node_status() -> dict: def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: config = load_product_config(PRODUCT, workspace=REPO, check_can=False) + assert (config.model, config.side, config.tag_layout) == ( + "G20", + "right", + G20_RIGHT_19_LAYOUT, + ) + assert config.calibration_contract.profile.command_count == 20 + assert config.calibration_contract.profile.supports( + "urdf_zero_publication" + ) + assert config.calibration_contract.profile.supports( + "stable_cross_view_cone_bias" + ) assert config.serial_number == "G20_RIGHT_001" assert config.required_independent_passes == 1 assert set(config.cameras) == {"front", "side", "top"} @@ -266,6 +298,71 @@ def test_resume_reuses_only_a_fully_committed_task_prefix() -> None: assert {row["task_name"] for row in reusable} == {first.key} +def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + spec = next( + item for item in profile.sweep_specs if item.key == "pinky_pitch_side" + ) + rows = _complete_resume_task_rows(profile, spec) + rows.append( + { + "kind": "palm_axis_sample", + "attempt": 1, + "task_name": spec.key, + "source_joint": "pinky_mcp_pitch_front_axis", + "cycle": 0, + "direction": "decreasing", + "feedback_u8": 240, + } + ) + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + allow_sparse=True, + ) + + assert completed == (spec.key,) + assert sum( + row.get("kind") == "palm_axis_sample" for row in reusable + ) == 1 + + completed_without_side_channel, _ = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + _complete_resume_task_rows(profile, spec), + allow_sparse=True, + ) + assert completed_without_side_channel == (spec.key,) + + +def test_checkpoint_needs_no_palm_axis_reacquisition_when_disabled() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + old_capabilities = set(profile.capabilities) - { + "palm_axis_side_channel_v1" + } + + compatible, invalidated = _palm_axis_resume_policy( + profile, {"capabilities": sorted(old_capabilities)} + ) + + assert compatible is True + assert invalidated == () + + compatible, invalidated = _palm_axis_resume_policy( + profile, + { + "capabilities": sorted(profile.capabilities), + "palm_axis_observers": _palm_axis_observer_schema(profile), + }, + ) + assert compatible is True + assert invalidated == () + + def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) first, failed, later = profile.sweep_specs[:3] @@ -590,6 +687,41 @@ def test_resume_revalidates_retired_passive_dip_position_metrics() -> None: assert _unresolved_fit_failure_tasks(profile, rows) == set() +def test_resume_retires_validation_only_cross_view_curve_failure() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + spec = next( + item for item in profile.sweep_specs + if item.key == "ring_roll_multiview" + ) + rows = [ + { + "kind": "sample", + "task_name": spec.key, + "attempt": 3, + }, + { + "kind": "fit_failure", + "task_name": spec.key, + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "attempt": 3, + "failures": [ + { + "joint": "ring_mcp_roll", + "metric": "cross_view_roll_curve", + "reason": ( + "cross_view_roll_curve_difference_too_large:" + "angle_rad:1.297957deg" + ), + } + ], + }, + ] + + assert _unresolved_fit_failure_tasks(profile, rows) == set() + + def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None: config = _config(tmp_path) session = config.session_root / "20260819_150000" @@ -718,6 +850,9 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( }, *_complete_resume_task_rows(profile, profile.sweep_specs[0]), ] + for row in rows: + if row.get("task_name") == profile.sweep_specs[0].key: + row["attempt"] = 2 source_raw.write_text( "".join(json.dumps(row) + "\n" for row in rows) ) @@ -760,10 +895,14 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( assert count == 1 assert node.sweep_index == 10 assert node.resumed_task_keys == (profile.sweep_specs[0].key,) + assert node.sweep_attempts[profile.sweep_specs[0].key] == 2 restored = [ json.loads(line) for line in current_raw.read_text().splitlines() ] assert restored[0]["kind"] == "resume_checkpoint_import" + assert restored[0]["imported_attempt_floor_by_task"] == { + profile.sweep_specs[0].key: 2 + } assert any(row["kind"] == "sample" for row in restored) @@ -839,10 +978,89 @@ def test_publication_numerically_binds_json_offsets_and_urdf_limits( payload = _payload(config.serial_number) validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) payload["joints"]["thumb_mcp"]["angle_rad"][0] = 2.0 - with pytest.raises(ValueError, match="exceeds source URDF limit"): + with pytest.raises(ValueError, match="exceeds runtime URDF limit"): validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) +def test_publication_uses_corrected_coordinates_for_negative_endpoint_zero( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} + endpoint_offset = -0.00801549 + offsets["index_mcp_pitch"] = endpoint_offset + session = _make_passed_session( + config, + "20260826_102518", + zero_offsets=offsets, + ) + paths = session_artifact_paths(session, config.serial_number) + payload = json.loads(paths["json"].read_text(encoding="utf-8")) + curve_maximum = 1.22801310 + payload["joints"]["index_mcp_pitch"]["angle_rad"][0] = curve_maximum + atomic_write_json(paths["json"], payload) + + summary, release_ready = finalize_session_artifacts( + config, + session, + node_status=_passed_node_status(), + ) + + def upper_limit(path: Path) -> float: + joint = next( + node + for node in ET.parse(path).getroot().findall("joint") + if node.get("name") == "index_mcp_pitch" + ) + return float(joint.find("limit").get("upper")) + + source_upper = upper_limit(config.source_urdf) + corrected_upper = upper_limit(paths["urdf"]) + assert release_ready is True + assert summary["result"] == "PASS" + assert corrected_upper == pytest.approx(source_upper - endpoint_offset) + assert curve_maximum <= corrected_upper + assert curve_maximum + endpoint_offset <= source_upper + + +def test_publication_propagates_schema_v4_rounding_to_mimic_offset( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} + full_precision_offset = -0.06200179363832 + offsets["middle_pip"] = full_precision_offset + session = _make_passed_session( + config, + "20260827_152432", + zero_offsets=offsets, + ) + paths = session_artifact_paths(session, config.serial_number) + payload = json.loads(paths["json"].read_text(encoding="utf-8")) + assert payload["joints"]["middle_pip"]["zero_angles"][ + "urdf_zero_offset_rad" + ] == -0.06200179 + + summary, release_ready = finalize_session_artifacts( + config, + session, + node_status=_passed_node_status(), + ) + + corrected = ET.parse(paths["urdf"]).getroot() + middle_dip = next( + node + for node in corrected.findall("joint") + if node.get("name") == "middle_dip" + ) + assert float(middle_dip.find("mimic").get("offset")) == pytest.approx( + 0.89 * full_precision_offset, + abs=1.0e-14, + ) + assert release_ready is True + assert summary["result"] == "PASS" + + def test_publication_rejects_non_joint_urdf_changes(tmp_path: Path) -> None: config = _config(tmp_path, passes=1) session = _make_passed_session(config, "20260819_112000") @@ -907,7 +1125,7 @@ def test_publication_accepts_half_lsb_schema_zero_quantisation( assert release_ready is True -def test_publication_saturates_legacy_runtime_curve_before_release( +def test_publication_keeps_cmc_runtime_limit_independent_of_certified_zero( tmp_path: Path, ) -> None: config = _config(tmp_path, passes=1) @@ -923,6 +1141,9 @@ def test_publication_saturates_legacy_runtime_curve_before_release( published = json.loads(json_path.read_text(encoding="utf-8")) assert release_ready is True + # CMC has a certified origin, not an assertion that its electrical + # endpoint equals the source-CAD upper coordinate. Its runtime coordinate + # limit therefore remains the protected source value. assert published["joints"]["thumb_cmc_yaw"]["angle_rad"][0] == 1.57 assert summary["runtime_limit_clipped_bins"]["thumb_cmc_yaw"] == 1 @@ -977,7 +1198,19 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P "front": { "ready": True, "missing_tag_ids": [], - "group_pnp_reason": "group_coupled_rotation", + "group_pnp_reason": "group_missing_pose_candidates", + "group_missing_candidate_roles": ["thumb_ip"], + "pnp_rejection_counts": { + "thumb_ip:no_pose_within_reprojection_or_tilt_limit": 12, + }, + "pnp_candidate_diagnostics": { + "thumb_ip": { + "tag_id": 3, + "solved_candidate_count": 2, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + } + }, }, "side": {"ready": True, "missing_tag_ids": []}, "top": {"ready": True, "missing_tag_ids": []}, @@ -989,7 +1222,12 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P ) assert payload["error_code"] == "CAM-GEOMETRY-201" + assert "轨迹在运动中失效" in payload["problem_zh"] assert "整组PnP几何检查拒绝" in payload["explanation_zh"] + assert "缺候选=thumb_ip" in payload["explanation_zh"] + assert payload["pnp_diagnostics"]["front"][ + "group_missing_candidate_roles" + ] == ["thumb_ip"] assert "不要调整" in payload["automatic_action_zh"] @@ -1007,7 +1245,14 @@ def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Pa "side": { "ready": True, "missing_tag_ids": [], - "group_pnp_reason": "group_normal_alignment", + "group_pnp_reason": "group_initializing:7/8", + "pnp_initialization_progress": { + "accepted": 7, + "required": 8, + }, + "group_pnp_rejection_counts": { + "group_normal_alignment": 5, + }, }, "top": {"ready": True, "missing_tag_ids": []}, }, @@ -1019,6 +1264,8 @@ def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Pa assert payload["error_code"] == "CAM-GEOMETRY-201" assert "PnP" in payload["problem_zh"] + assert "初始化7/8" in payload["explanation_zh"] + assert "group_normal_alignment×5" in payload["explanation_zh"] def test_multiview_sample_failure_has_stable_observation_code() -> None: @@ -1030,6 +1277,33 @@ def test_multiview_sample_failure_has_stable_observation_code() -> None: assert "轨迹不完整" in problem +def test_palm_orientation_coverage_has_validation_error_code() -> None: + code, problem, suggestion = classify_error( + "palm_orientation_quality_failed", {} + ) + + assert code == "VAL-QUALITY-501" + assert "方向校正" in problem + assert "至少三枚" in suggestion + + +@pytest.mark.parametrize( + "reason", + ( + "name 'endpoint_zero_offsets' is not defined", + "validated_endpoint_zero_state_incomplete:missing=index_pip;extra=-", + ), +) +def test_endpoint_zero_lifecycle_failure_is_a_publication_error( + reason: str, +) -> None: + code, problem, suggestion = classify_error(reason, {}) + + assert code == "PUB-ARTIFACT-601" + assert "发布阶段" in problem + assert "不要移动相机或Tag" in suggestion + + def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None: assert _status_timeout_seconds({"state": "FITTING"}) == 600.0 assert _status_timeout_seconds({"state": "SWEEP"}) == 90.0 @@ -1114,8 +1388,9 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: "actual_u8": 116.5, "valid_frames": 80, "automatic_retry_count": 1, - "fit_attempt": 3, - "fit_attempt_limit": 3, + "fit_attempt": 3, + "fit_attempt_limit": 3, + "fit_retry_cycles": [2], }, "views": { view: { @@ -1136,7 +1411,7 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: assert "反馈：98.0 Hz" in text assert "当前任务ID：正面[0✓] 侧面[0✓] 顶部[0✓]" in text assert "断点：已恢复 3/16 个完整任务" in text - assert "拟合：整关节第 3/3 次尝试" in text + assert "首次拟合定位第 2 轮异常，仅补采该轮双向" in text def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None: @@ -1218,7 +1493,7 @@ def test_sweep_progress_lists_visible_and_missing_task_tag_ids() -> None: "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) ) - assert "Tag：4/5 有效" in text + assert "Tag：4/5 可见" in text assert "正面[0✓]" in text assert "侧面[4✓,5✗,14✓]" in text assert "顶部[8✓]" in text @@ -1254,6 +1529,14 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None: "required_tag_ids": [4, 15], "locked_reference_tag_ids": [], "detected_tag_ids": [4, 15], + "pnp_pose_valid": False, + "pnp_initialization_progress": { + "accepted": 5, + "required": 8, + }, + "pnp_rejection_counts": { + "middle_pip:no_pose_within_reprojection_or_tilt_limit": 7, + }, }, "top": { "camera_info_valid": True, @@ -1271,9 +1554,11 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None: assert "已到扫描起点，等待任务Tag" in text assert "命令/反馈：255/254.0" in text - assert "Tag：5/5 有效（含锁定 1）" in text + assert "Tag：5/5 可见（含锁定 1）" in text assert "正面[0锁,12✓]" in text assert "预计剩余 等待Tag" in text + assert "PnP：侧面 初始化5/8" in text + assert "Tag可见不等于三维位姿有效" in text def test_device_preflight_defers_tag_gate_until_after_baseline_recovery() -> None: diff --git a/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py b/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py index 6173ee2..5837d58 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py +++ b/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py @@ -2,6 +2,7 @@ import pytest from g20_thumb_apriltag_calibration.offline_replay import ( _latest_attempt_records, + _latest_palm_axis_records, _output_suffix, ) @@ -36,6 +37,30 @@ def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None: ] == [1] +def test_latest_palm_axis_attempt_is_selected_per_cycle_and_direction() -> None: + def sample(cycle: int, direction: str, attempt: int) -> dict: + return { + "kind": "palm_axis_sample", + "source_joint": "pinky_mcp_pitch_front_axis", + "cycle": cycle, + "direction": direction, + "attempt": attempt, + } + + selected = _latest_palm_axis_records( + [ + sample(0, "decreasing", 1), + sample(0, "decreasing", 2), + sample(0, "increasing", 1), + ] + ) + + assert [ + record["attempt"] + for record in selected["pinky_mcp_pitch_front_axis"] + ] == [2, 1] + + def test_output_suffix_is_safe_and_explicit() -> None: assert _output_suffix(None) == "" assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2" diff --git a/src/g20_thumb_apriltag_calibration/test/test_pnp.py b/src/g20_thumb_apriltag_calibration/test/test_pnp.py index 050b3d2..2f358d4 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_pnp.py +++ b/src/g20_thumb_apriltag_calibration/test/test_pnp.py @@ -234,6 +234,83 @@ def _pose( ) +def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None: + per_tag = SquareTagPoseTracker( + maximum_reprojection_error_px=1.5, + reprojection_tie_px=1.5, + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + maximum_tag_tilt_rad=np.deg2rad(75.0), + reset_after_seconds=5.0, + ) + oblique_rotation = Rotation.from_euler("y", 89.0, degrees=True) + + independent, reason = per_tag.estimate( + "child", + _project(oblique_rotation, np.asarray([0.03, 0.0, 0.25])), + tag_size_m=0.01, + camera_matrix=_camera_matrix(), + stamp_ns=1_000_000_000, + ) + + assert independent is None + assert reason == "no_pose_within_reprojection_or_tilt_limit" + assert per_tag.last_candidates_by_role["child"] + diagnostics = per_tag.last_candidate_diagnostics_by_role["child"] + assert diagnostics["reprojection_candidate_count"] == 2 + assert diagnostics["independent_tilt_candidate_count"] == 0 + assert diagnostics["minimum_candidate_tilt_deg"] > 75.0 + + group = SquareTagGroupPoseTracker( + roles=("base", "child"), + adjacent_pairs=(("base", "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, + ) + selected, group_reason = group.select( + { + "base": (_pose(0.0, 0.0, 0.05),), + "child": per_tag.last_candidates_by_role["child"], + }, + stamp_ns=1_000_000_000, + ) + + assert group_reason == "" + assert selected is not None + assert "child" in selected + + +def test_group_tracker_exposes_roles_without_candidates() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("base", "parent", "child"), + adjacent_pairs=(("base", "parent"), ("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, + ) + + selected, reason = tracker.select( + { + "base": (_pose(0.0, 0.0, 0.05),), + "parent": (), + }, + stamp_ns=1_000_000_000, + ) + + assert selected is None + assert reason == "group_missing_pose_candidates" + assert tracker.last_missing_roles == ("parent", "child") + + def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None: tracker = SquareTagGroupPoseTracker( roles=("base", "mcp", "ip"), diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py index 9124739..c17a504 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py @@ -74,6 +74,47 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None: assert "/g20_calibration/resume" in text +def test_endpoint_zero_lifecycle_failure_is_not_reported_as_quality() -> None: + payload = { + "state": "PAUSED", + "reason": ( + "validated_endpoint_zero_state_incomplete:" + "missing=index_pip;extra=-" + ), + "active": {}, + "progress": 1.0, + "completed_sweeps": 16, + "total_sweeps": 16, + "views": {}, + } + + text = render_three_camera_status_text_zh(payload) + + assert "程序内部状态生命周期错误" in text + assert "不要移动相机、Tag或机械手底座" in text + + +def test_artifact_failure_explains_that_collection_need_not_restart() -> None: + payload = { + "state": "PAUSED", + "reason": ( + "PUB-ARTIFACT-601:runtime curve exceeds runtime URDF limit " + "for index_mcp_pitch" + ), + "active": {}, + "progress": 1.0, + "completed_sweeps": 16, + "total_sweeps": 16, + "views": {}, + } + + text = render_three_camera_status_text_zh(payload) + + assert "正式发布前" in text + assert "原始URDF未被覆盖" in text + assert "不要重新标定相机或调整Tag" in text + + def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None: explanation, action = three_camera_reason_zh( "PAUSED", "combination_pose_prediction_failed", {} @@ -91,14 +132,58 @@ def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None: "sweep_start_tag_timeout", { "group_pnp_reasons": { - "side": "group_normal_alignment", - } + "side": "group_initializing:5/8", + }, + "pnp_initialization_progress": { + "side": {"accepted": 5, "required": 8}, + }, + "pnp_rejection_counts": { + "side": { + "ring_pip:no_pose_within_reprojection_or_tilt_limit": 12, + }, + }, }, ) assert "所需Tag也可见" in explanation - assert "group_normal_alignment" in explanation - assert "不要重新粘贴" in action + assert "初始化5/8" in explanation + assert "no_pose_within_reprojection_or_tilt_limit×12" in explanation + assert "不要根据可见性重复粘贴" in action + + +def test_mid_sweep_pnp_failure_names_missing_candidate_role() -> None: + explanation, action = three_camera_reason_zh( + "PAUSED", + "synchronised_tag_state_timeout:side", + { + "valid_frames": 150, + "group_pnp_reasons": { + "side": "group_missing_pose_candidates", + }, + "group_missing_candidate_roles": { + "side": ["index_dip"], + }, + "pnp_rejection_counts": { + "side": { + "index_dip:no_pose_within_reprojection_or_tilt_limit": 90, + }, + }, + "pnp_candidate_diagnostics": { + "side": { + "index_dip": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + }, + }, + }, + }, + ) + + assert "已经取得部分有效轨迹" in explanation + assert "缺候选=index_dip" in explanation + assert "solve=2,reproj=0,tilt=0" in explanation + assert "group_pnp_candidate_event" in action def test_multiview_failure_names_the_camera_specific_joint() -> None: @@ -183,6 +268,60 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None: assert "运动采样：" not in text +def test_palm_orientation_failure_explains_direction_coverage() -> None: + explanation, suggestion = three_camera_reason_zh( + "PAUSED", + "palm_orientation_quality_failed", + { + "failures": [ + { + "reason": "palm orientation cycle 2 has 2/3 usable sources" + } + ] + }, + ) + + assert "至少三根手指" in explanation + assert "2/3 usable sources" in explanation + assert "Tag 10–13" in suggestion + + +def test_cross_view_side_line_rms_names_side_source() -> None: + payload = { + "state": "PAUSED", + "reason": "joint_fit_check_failed", + "progress": 0.1, + "completed_sweeps": 8, + "total_sweeps": 80, + "active": { + "kind": "fit_failure", + "view": "front", + "motor_index": 9, + "joints": ["pinky_mcp_roll", "pinky_mcp_roll_side"], + "attempt": 1, + "directions_to_rescan": 8, + "failures": [ + { + "joint": "pinky_mcp_roll_side", + "model_joint": "pinky_mcp_roll", + "quality_source_joints": ["pinky_mcp_roll_side"], + "metric": "axis_line_cycle_rms_mm", + "actual": 1.2, + "limit": 1.0, + "comparison": "maximum", + } + ], + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "小指MCP侧摆（侧面校验）的四轮轴线位置RMS为1.20mm" in text + assert "要求不超过1.00mm" in text + + def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None: payload = { "state": "PAUSED", @@ -297,7 +436,7 @@ def test_zero_model_failure_explains_that_rescan_will_not_help() -> None: assert "运动采样：" not in text -def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None: +def test_sweep_status_shows_localized_fit_retry_cycle() -> None: active = { "kind": "sweep", "view": "front", @@ -309,9 +448,10 @@ def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None: "target_u8": 0, "fit_attempt": 2, "fit_attempt_limit": 3, + "fit_retry_cycles": [2], } - assert "整关节自动重采第2/3次" in _task_text(active) + assert "补采异常轮2" in _task_text(active) def test_motor_stall_reason_is_explained_in_chinese() -> None: diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py index 50da502..296b52e 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py @@ -8,12 +8,15 @@ import numpy as np import pytest from scipy.spatial.transform import Rotation +import g20_thumb_apriltag_calibration.three_camera_node as three_camera_node from g20_thumb_apriltag_calibration.core import ( DIRECTION_DECREASING, DIRECTION_INCREASING, ) from g20_thumb_apriltag_calibration.acquisition import TagQuality from g20_thumb_apriltag_calibration.full_hand import ( + JointCurveFit, + PalmAxisObserver, RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, RIGHT_19_HAND_PROFILE, RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, @@ -27,20 +30,33 @@ from g20_thumb_apriltag_calibration.full_hand import ( from g20_thumb_apriltag_calibration.three_camera_node import ( FrameObservation, G20ThreeCameraCalibrationNode, + STATE_COMPLETE, STATE_PREPARE_SWEEP, STATE_SWEEP, SweepItem, + _build_sweep_plan, _classify_cross_view_roll_hysteresis, + _cross_view_curve_failure, _combination_observable_joints, _fit_failure_is_systematic, + _fit_retry_joint_names, _frames_cover_sweep_joints, _isolated_axis_cycle_outliers, + _isolated_axis_line_cycle_outliers, _overall_progress, + _preserve_pnp_task_reference_for_sweep, + _palm_axis_observer_for_sweep, + _requires_pnp_tracker_reset_for_sweep, _selected_pose_qualities, + _sweep_plan_transition, _sweep_views, ) from g20_thumb_apriltag_calibration.pnp import SquareTagPose from g20_thumb_apriltag_calibration.urdf_zero import ( + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + JointAxisMeasurement, + axis_line_cycle_rms_m, + cross_view_side_line_source, get_zero_calibration_profile, ) @@ -54,6 +70,390 @@ def _sweep_items() -> list[SweepItem]: ] +def _validated_zero_result( + endpoint_offsets: dict[str, float], +) -> SimpleNamespace: + all_offsets = { + name: float(endpoint_offsets.get(name, 0.0)) + for name in RIGHT_19_HAND_PROFILE.active_joints + } + return SimpleNamespace( + passed=True, + direct_offsets_rad=dict(all_offsets), + all_active_offsets_rad=dict(all_offsets), + offset_confidence_half_width_rad={}, + cycle_offsets_rad={}, + training_cycles=(0, 1, 2), + validation_cycle=3, + axis_line_rms_m=0.0001, + validation_line_error_by_joint_m={}, + observability_rank=22, + observability_parameter_count=22, + observability_condition_number=100.0, + offset_covariance_rad2={}, + ) + + +def test_finalize_publishes_the_frozen_validated_endpoint_state( + tmp_path, monkeypatch +) -> None: + endpoint_offsets = { + name: 0.001 * (index + 1) + for index, name in enumerate( + sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) + ) + } + zero_result = _validated_zero_result(endpoint_offsets) + session_dir = tmp_path / "20260826_100727" + session_dir.mkdir() + source_urdf = tmp_path / "source.urdf" + source_urdf.write_text("", encoding="utf-8") + extrinsics = tmp_path / "extrinsics.yaml" + extrinsics.write_text("test: true\n", encoding="utf-8") + corrected_urdf = tmp_path / "corrected.urdf" + captured: dict[str, object] = {} + + def fake_write_zero_corrected_urdf(**kwargs): + captured.update(kwargs) + corrected_urdf.write_text("", encoding="utf-8") + return corrected_urdf + + monkeypatch.setattr( + three_camera_node, + "write_zero_corrected_urdf", + fake_write_zero_corrected_urdf, + ) + monkeypatch.setattr( + three_camera_node, + "build_compact_payload", + lambda **kwargs: {"passed": kwargs["passed"]}, + ) + monkeypatch.setattr( + three_camera_node, + "atomic_write_json", + lambda path, payload: captured.update( + {"final_path": path, "payload": payload} + ), + ) + node = SimpleNamespace( + zero_result=zero_result, + validated_endpoint_zero_offsets_rad=dict(endpoint_offsets), + validation_errors_rad=[], + combination_validation_enabled=False, + combination_validation_completed=False, + validation_enabled=False, + fit_quality_passed=True, + session_dir=session_dir, + profile=RIGHT_19_HAND_PROFILE, + zero_profile=get_zero_calibration_profile("right", "g20_right_19"), + source_urdf_path=source_urdf, + corrected_urdf_output_dir=tmp_path, + serial_number="G20_RIGHT_001", + measured_fits={}, + baseline_command=THREE_CAMERA_BASELINE_COMMAND, + hand_type="right", + camera_extrinsics_file=extrinsics, + final_path=session_dir / "calibration.json", + completed_payload=None, + state="RETURN_BASELINE", + reason="finalize", + get_logger=lambda: SimpleNamespace(info=lambda message: None), + ) + + G20ThreeCameraCalibrationNode._finalize(node) + + assert captured["endpoint_anchored_offsets_rad"] == endpoint_offsets + assert captured["payload"] == {"passed": True} + assert node.state == STATE_COMPLETE + assert node.corrected_urdf_path == corrected_urdf + + +def test_endpoint_state_must_match_the_validated_zero_solution() -> None: + endpoint_offsets = { + name: 0.001 + for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + } + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + zero_result=_validated_zero_result(endpoint_offsets), + validated_endpoint_zero_offsets_rad={ + name: value + for name, value in endpoint_offsets.items() + if name != "index_pip" + }, + ) + + with pytest.raises( + RuntimeError, match="validated_endpoint_zero_state_incomplete" + ): + G20ThreeCameraCalibrationNode._endpoint_zero_offsets_for_publication( + node + ) + + +def test_refit_invalidates_all_derived_calibration_artifacts() -> None: + node = SimpleNamespace( + measured_fits={"index_pip": object()}, + axis_measurements=[object()], + zero_result=object(), + validated_endpoint_zero_offsets_rad={"index_pip": 0.01}, + corrected_urdf_path=object(), + fit_quality_passed=True, + validation_errors_rad=[0.01], + cross_view_roll_metrics={"index_mcp_roll": {"rms": 0.01}}, + validation_only_fits={"index_mcp_roll_side": object()}, + joint_dynamic_diagnostics={"index_pip": {"holdout": 0.01}}, + ) + + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(node) + + assert node.measured_fits == {} + assert node.axis_measurements == [] + assert node.zero_result is None + assert node.validated_endpoint_zero_offsets_rad == {} + assert node.corrected_urdf_path is None + assert node.fit_quality_passed is False + assert node.validation_errors_rad == [] + assert node.cross_view_roll_metrics == {} + assert node.validation_only_fits == {} + assert node.joint_dynamic_diagnostics == {} + + +def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None: + plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) + + assert len(plan) == len(RIGHT_19_HAND_PROFILE.sweep_specs) * 10 + for spec_index, spec in enumerate(RIGHT_19_HAND_PROFILE.sweep_specs): + task = plan[spec_index * 10 : (spec_index + 1) * 10] + assert all(item.spec == spec for item in task) + assert [ + (item.precheck, item.cycle, item.direction) + for item in task + ] == [ + (True, -1, DIRECTION_DECREASING), + (True, -1, DIRECTION_INCREASING), + *[ + (False, cycle, direction) + for cycle in range(4) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ], + ] + transitions = [ + _sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right) + for left, right in zip(task, task[1:]) + ] + assert transitions == [ + "immediate_reverse", + "immediate_reverse", + "immediate_reverse", + "cycle_reset", + "immediate_reverse", + "cycle_reset", + "immediate_reverse", + "cycle_reset", + "immediate_reverse", + ] + task_boundaries = [ + (plan[index], plan[index + 1]) + for index in range(9, len(plan) - 1, 10) + ] + assert all( + _sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right) + == "task_change" + for left, right in task_boundaries + ) + + +def test_right_19_complete_plan_has_no_unobserved_immediate_handoff() -> None: + """Every continuous boundary inherits the endpoint it already observed.""" + plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) + immediate_boundaries = 0 + cycle_resets = 0 + task_changes = 0 + + for completed, following in zip(plan, plan[1:]): + transition = _sweep_plan_transition( + RIGHT_19_HAND_PROFILE, completed, following + ) + if transition == "cycle_reset": + cycle_resets += 1 + continue + if transition == "task_change": + task_changes += 1 + continue + + immediate_boundaries += 1 + endpoint_state = list( + build_calibration_motion_command( + completed.spec, + completed.target_u8, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + ) + ) + terminal_frames = [ + _frame(view, endpoint_state) + for view in _sweep_views( + RIGHT_19_HAND_PROFILE, completed.spec + ) + ] + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + sweep_frames=terminal_frames, + carried_sweep_start_frames=[], + preflight_frames=30, + raw_path=None, + synchronised_endpoint_tolerance_margin_u8=1.0, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + ) + + carried = ( + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + node, completed, following + ) + ) + + assert carried == len(terminal_frames) + assert node.carried_sweep_start_frames == terminal_frames + assert _frames_cover_sweep_joints( + node.carried_sweep_start_frames, + following.spec, + minimum_per_joint=1, + ) + + # 16 tasks × (precheck out/back, precheck->formal, four formal reversals). + assert immediate_boundaries == 16 * 6 + assert cycle_resets == 16 * 3 + assert task_changes == 15 + + +def test_right_19_initializes_pnp_once_per_normal_task() -> None: + plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) + reset_items = [ + item + for item in plan + if _requires_pnp_tracker_reset_for_sweep( + RIGHT_19_HAND_PROFILE, + item, + is_fit_retry=False, + ) + ] + + assert len(reset_items) == len(RIGHT_19_HAND_PROFILE.sweep_specs) + assert all(item.precheck for item in reset_items) + assert all(item.cycle == -1 for item in reset_items) + assert all( + item.direction == DIRECTION_DECREASING for item in reset_items + ) + + +def test_pnp_reset_policy_preserves_legacy_and_retry_recovery() -> None: + right_spec = RIGHT_19_HAND_PROFILE.sweep_specs[0] + legacy_spec = RIGHT_HAND_PROFILE.sweep_specs[0] + + assert not _requires_pnp_tracker_reset_for_sweep( + RIGHT_19_HAND_PROFILE, + SweepItem(right_spec, 2, DIRECTION_DECREASING), + is_fit_retry=False, + ) + assert _requires_pnp_tracker_reset_for_sweep( + RIGHT_19_HAND_PROFILE, + SweepItem(right_spec, 2, DIRECTION_DECREASING), + is_fit_retry=True, + ) + assert _requires_pnp_tracker_reset_for_sweep( + RIGHT_HAND_PROFILE, + SweepItem(legacy_spec, 2, DIRECTION_DECREASING), + is_fit_retry=False, + ) + + +def test_task_roles_remain_owned_during_intercycle_reset() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "ring_pitch_side" + ) + view = "side" + all_roles = tuple(RIGHT_19_HAND_PROFILE.view_tags[view]) + runtime = SimpleNamespace( + preflight_roles=all_roles, + roles=all_roles, + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={view: runtime}, + active_combination_validation=None, + active_sweep=None, + active_validation=None, + retry_sweep_spec=None, + pnp_task_spec=spec, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, view + ) + + assert required == ("side_base", "ring_pip") + assert required != all_roles + + +def _linear_curve_fit(offset_scale_rad: float = 0.0) -> JointCurveFit: + base = np.asarray( + [0.5 * (127 - command) / 127 for command in range(256)], + dtype=float, + ) + shape = np.abs(np.arange(256, dtype=float) - 127.0) / 128.0 + values = tuple(float(value) for value in base + offset_scale_rad * shape) + return JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + + +def test_cross_view_curve_stable_four_cycle_bias_is_not_rescanned() -> None: + primary = _linear_curve_fit() + biased = _linear_curve_fit(math.radians(1.8)) + + failure = _cross_view_curve_failure( + "pinky_mcp_roll", + "pinky_mcp_roll_side", + primary, + biased, + scope="all_cycles", + maximum_rms_difference_rad=math.radians(1.0), + maximum_branch_gap_difference_rad=math.radians(0.5), + cycle_fits=[(primary, biased)] * 4, + ) + + assert failure is not None + assert failure["systematic"] is True + assert failure["quality_source_joints"] == ["pinky_mcp_roll_side"] + assert "cycle" not in failure + assert _fit_failure_is_systematic([failure], 4) is True + + +def test_cross_view_curve_single_bad_cycle_localizes_side_retry() -> None: + primary = _linear_curve_fit() + good = _linear_curve_fit(math.radians(0.6)) + bad = _linear_curve_fit(math.radians(1.9)) + + failure = _cross_view_curve_failure( + "middle_mcp_roll", + "middle_mcp_roll_side", + primary, + bad, + scope="all_cycles", + maximum_rms_difference_rad=math.radians(1.0), + maximum_branch_gap_difference_rad=math.radians(0.5), + cycle_fits=[(primary, good)] * 3 + [(primary, bad)], + ) + + assert failure is not None + assert failure["cycle"] == 4 + assert failure.get("systematic", False) is False + + def _frame(view: str, state_u8: list[float]) -> FrameObservation: return FrameObservation( stamp_ns=1, @@ -135,6 +535,101 @@ def test_right_roll_requires_both_views_task_local_tags() -> None: ) == ("side_base", "pinky_pip") +def test_pitch_task_does_not_add_a_front_direction_observer() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_pitch_side" + ) + + assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("side",) + observer = _palm_axis_observer_for_sweep( + RIGHT_19_HAND_PROFILE, spec, "front" + ) + assert observer is None + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={ + view: SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags[view]), + preflight_roles=(f"{view}_base",), + ) + for view in ("front", "side", "top") + }, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + active_combination_validation=None, + ) + assert G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "front" + ) == ("front_base",) + assert G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) == ("side_base", "pinky_pip") + + +def test_disabled_palm_axis_side_channel_is_not_persisted(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_pitch_side" + ) + observer = _palm_axis_observer_for_sweep( + RIGHT_19_HAND_PROFILE, spec, "front" + ) + assert observer is None + observer = PalmAxisObserver( + source_name="pinky_mcp_pitch_front_axis", + task_name=spec.key, + view="front", + parent_role="front_base", + child_role="pinky_roll", + model_joint="pinky_mcp_pitch", + motor_index=9, + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + active_sweep=item, + sweep_attempts={spec.key: 1}, + palm_axis_records_by_source={observer.source_name: []}, + raw_path=tmp_path / "raw_samples.jsonl", + ) + selected = { + "front_base": SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 + ), + "pinky_roll": SquareTagPose( + tuple( + Rotation.from_euler("y", 12.0, degrees=True).as_quat() + ), + (0.02, 0.0, 1.0), + 0.2, + ), + } + state = list(THREE_CAMERA_BASELINE_COMMAND) + state[observer.motor_index] = 230.0 + + G20ThreeCameraCalibrationNode._record_palm_axis_sample( + node, + observer, + selected, + np.eye(4), + state, + sync_error_ns=100_000, + stamp_ns=123, + ) + + records = node.palm_axis_records_by_source[observer.source_name] + assert len(records) == 1 + assert records[0]["kind"] == "palm_axis_sample" + assert "joint" not in records[0] + assert records[0]["command_u8"] == 230 + G20ThreeCameraCalibrationNode._persist_palm_axis_samples(node, item) + assert not node.raw_path.exists() + + def test_multiview_roll_uses_locked_front_base_but_live_moving_tag() -> None: spec = next( item @@ -518,6 +1013,88 @@ def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None assert required == ("side_base", "index_pip", "index_dip") +def test_group_pnp_failure_event_persists_candidate_boundary_once( + tmp_path, +) -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs if item.key == "index_pip_side" + ) + state_u8 = [255.0] * 20 + state_u8[spec.motor_index] = 195.0 + runtime = SimpleNamespace( + name="side", + tracker=SimpleNamespace( + last_candidate_diagnostics_by_role={ + "index_pip": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 2, + "independent_tilt_candidate_count": 2, + }, + "index_dip": { + "solved_candidate_count": 2, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "minimum_reprojection_error_px": 1.7, + }, + } + ), + latest_group_missing_candidate_roles=("index_dip",), + last_pnp_diagnostic_signature=None, + view_tags=profile.view_tags["side"], + intrinsics_sha256="side-intrinsics", + camera_matrix=np.eye(3), + ) + node = SimpleNamespace( + profile=profile, + active_sweep=SweepItem( + spec, -1, DIRECTION_DECREASING, precheck=True + ), + state=STATE_SWEEP, + latest_state_u8=tuple(state_u8), + raw_path=tmp_path / "raw_samples.jsonl", + ) + arguments = dict( + runtime=runtime, + stamp_ns=123, + required_roles=("side_base", "index_pip", "index_dip"), + pose_roles=("index_pip", "index_dip"), + corners_by_role={ + "index_pip": np.zeros((4, 2)), + "index_dip": np.ones((4, 2)), + }, + qualities={ + "index_pip": TagQuality(0, 80.0, 40.0), + "index_dip": TagQuality(0, 70.0, 35.0), + }, + matched_tracking=(tuple(state_u8), 2_000_000), + selected=None, + pnp_rejections={ + "index_dip": "no_pose_within_reprojection_or_tilt_limit" + }, + group_pnp_reason="group_missing_pose_candidates", + ) + + G20ThreeCameraCalibrationNode._record_group_pnp_candidate_event( + node, **arguments + ) + G20ThreeCameraCalibrationNode._record_group_pnp_candidate_event( + node, **arguments + ) + + rows = [ + json.loads(line) + for line in node.raw_path.read_text(encoding="utf-8").splitlines() + ] + assert len(rows) == 1 + assert rows[0]["kind"] == "group_pnp_candidate_event" + assert rows[0]["feedback_u8"] == 195.0 + assert rows[0]["group_missing_candidate_roles"] == ["index_dip"] + assert rows[0]["candidate_diagnostics"]["index_dip"][ + "reprojection_candidate_count" + ] == 0 + + def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None: profile = RIGHT_HAND_PROFILE spec = next( @@ -614,6 +1191,54 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None: assert published[0][7:10] == [0, 0, 0] +def test_immediate_reverse_reuses_proven_terminal_endpoint_frames(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 1 + ) + completed = SweepItem( + spec, -1, DIRECTION_DECREASING, precheck=True + ) + following = SweepItem( + spec, -1, DIRECTION_INCREASING, precheck=True + ) + endpoint_state = list( + build_calibration_motion_command( + spec, + 0, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + ) + ) + endpoint_state[1] = 2.5 + outside_state = list(endpoint_state) + outside_state[1] = 3.01 + endpoint_frame = _frame("side", endpoint_state) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + sweep_frames=[endpoint_frame, _frame("side", outside_state)], + carried_sweep_start_frames=[], + preflight_frames=30, + raw_path=tmp_path / "raw_samples.jsonl", + synchronised_endpoint_tolerance_margin_u8=1.0, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + ) + + count = ( + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + node, completed, following + ) + ) + + assert count == 1 + assert node.carried_sweep_start_frames == [endpoint_frame] + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["kind"] == "sweep_endpoint_frames_carried" + assert event["endpoint_u8"] == 0 + assert event["to_direction"] == DIRECTION_INCREASING + + def test_right_19_roll_sweep_stops_at_settled_baseline_first() -> None: spec = next( item @@ -1382,11 +2007,11 @@ def test_retry_return_anchors_roll_order_on_failed_target_finger() -> None: def test_precheck_dense_coverage_selects_faster_formal_speed(tmp_path) -> None: spec = next( item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_pitch_front" + for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 0 ) node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, + profile=RIGHT_HAND_PROFILE, active_sweep=None, normal_calibration_speed=15, index_roll_calibration_speed=5, @@ -1421,6 +2046,49 @@ def test_precheck_dense_coverage_selects_faster_formal_speed(tmp_path) -> None: assert event["formal_speed"] == 22 +def test_right_19_dense_precheck_keeps_deterministic_formal_speed(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "thumb_cmc_pitch_front" + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + active_sweep=None, + normal_calibration_speed=15, + index_roll_calibration_speed=5, + index_flex_calibration_speed=10, + adaptive_formal_speed_enabled=True, + adaptive_formal_speed_max_scale=1.5, + adaptive_formal_speed_minimum_bins=64, + adaptive_formal_speed_maximum_bin_gap=8, + precheck_speed_metrics={}, + formal_speed_scales={}, + sweep_retry_counts={}, + raw_path=tmp_path / "raw_samples.jsonl", + ) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): + item = SweepItem(spec, -1, direction, precheck=True) + node.active_sweep = item + G20ThreeCameraCalibrationNode._record_precheck_speed_metric( + node, + item, + bin_count=220, + maximum_bin_gap=3, + valid_frames=380, + ) + + assert node.formal_speed_scales[spec.key] == 1.0 + node.active_sweep = SweepItem(spec, 0, DIRECTION_DECREASING) + speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) + assert speeds[0] == 15 + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["stability_speed_lock"] is True + assert event["ineligible_reason"] == ( + "g20_right_deterministic_acquisition_speed" + ) + + def test_precheck_large_gap_keeps_conservative_formal_speed(tmp_path) -> None: spec = next( item @@ -1495,7 +2163,9 @@ def test_roll_precheck_never_accelerates_strict_backlash_scan(tmp_path) -> None: assert node.formal_speed_scales[spec.key] == 1.0 event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) assert event["eligible"] is False - assert event["ineligible_reason"] == "roll_baseline_hysteresis_sensitive" + assert event["ineligible_reason"] == ( + "g20_right_deterministic_acquisition_speed" + ) def test_fit_retry_uses_base_speed_instead_of_adaptive_speed() -> None: @@ -1552,9 +2222,9 @@ def test_thumb_yaw_recovery_zero_accepts_feedback_four() -> None: def _image_cycle_records( - travels_rad: list[float], *, depth_slope: float = 0.0 + travels_rad: list[float], *, depth_slope: float = 0.0, command_step: int = 16 ) -> list[dict]: - commands = list(range(0, 256, 16)) + [255] + commands = list(range(0, 256, command_step)) + [255] records = [] for cycle, travel in enumerate(travels_rad): for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): @@ -1651,8 +2321,17 @@ def test_right_19_end_on_flexion_uses_image_curve_not_planar_pnp_tilt() -> None: ) record["relative_quaternion_xyzw"] = rotation.as_quat().tolist() - node = _fit_check_node({"pinky_pip": records, "pinky_dip": records}) + node = _fit_check_node( + { + "pinky_mcp_roll": records, + "pinky_pip": records, + "pinky_dip": records, + } + ) node.profile = RIGHT_19_HAND_PROFILE + roll_fit = G20ThreeCameraCalibrationNode._fit_joint_records( + node, "pinky_mcp_roll", records + ) image_fit = G20ThreeCameraCalibrationNode._fit_joint_records( node, "pinky_pip", records ) @@ -1661,6 +2340,7 @@ def test_right_19_end_on_flexion_uses_image_curve_not_planar_pnp_tilt() -> None: ) assert "pinky_pip" in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + assert roll_fit.circle["space"] == "image_2d" assert image_fit.circle["space"] == "image_2d" assert image_fit.quality["radial_rms_px"] < 1.0e-8 # DIP is measured against its moving PIP parent, so it must remain a @@ -1668,6 +2348,39 @@ def test_right_19_end_on_flexion_uses_image_curve_not_planar_pnp_tilt() -> None: assert passive_fit.circle["space"] == "relative_rotation_3d" +def test_right_19_finger_rolls_share_reference_axis_direction() -> None: + pinky = _image_cycle_records([math.radians(47.0)] * 3) + ring = _image_cycle_records([math.radians(47.0)] * 3) + for record in ring: + command = int(record["command_u8"]) + angle = math.radians(47.0) * (255.0 - command) / 255.0 + record["relative_quaternion_xyzw"] = ( + Rotation.from_rotvec([angle, 0.0, 0.0]).as_quat().tolist() + ) + + node = _fit_check_node( + {"pinky_mcp_roll": pinky, "ring_mcp_roll": ring} + ) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", RIGHT_19_HAND_PROFILE.layout_id + ) + reference = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + node, "pinky_mcp_roll", 0 + ) + measured = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + node, "ring_mcp_roll", 0 + ) + + assert measured.axis_direction_source == "upstream_constraint" + assert abs( + float( + np.asarray(reference.axis_common_xyz) + @ np.asarray(measured.axis_common_xyz) + ) + ) > math.cos(math.radians(1.0e-4)) + + def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None: node = SimpleNamespace( profile=RIGHT_19_HAND_PROFILE, @@ -1696,7 +2409,7 @@ def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None: ) -def test_side_pip_group_allows_arbitrary_distal_extension_tag_plane() -> None: +def test_side_joint_group_does_not_assume_tag_mounting_planes() -> None: node = SimpleNamespace( profile=RIGHT_19_HAND_PROFILE, pnp_maximum_pose_jump_rad=math.radians(35.0), @@ -1720,9 +2433,7 @@ def test_side_pip_group_allows_arbitrary_distal_extension_tag_plane() -> None: ("side_base", "middle_pip"), ("middle_pip", "middle_dip"), ) - assert tracker.normal_alignment_pairs == ( - ("side_base", "middle_pip"), - ) + assert tracker.normal_alignment_pairs == () def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None: @@ -1911,6 +2622,7 @@ def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None task_valid_frames=4927, task_total_frames=5212, ) + resets: list[bool] = [] node = SimpleNamespace( profile=RIGHT_19_HAND_PROFILE, retry_sweep_spec=spec, @@ -1933,7 +2645,11 @@ def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None ) ], views={"side": side_runtime}, - _reset_view_trackers=lambda runtime: None, + _reset_view_trackers=( + lambda runtime, preserve_task_reference=False: resets.append( + preserve_task_reference + ) + ), ) G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) @@ -1948,6 +2664,7 @@ def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None assert node.retry_joint_names == {validation} assert side_runtime.task_valid_frames == 0 assert side_runtime.task_total_frames == 0 + assert resets == [True] event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) assert event["joints_to_rescan"] == [validation] assert event["cycles"] == [1] @@ -2048,6 +2765,132 @@ def test_visibility_precheck_does_not_require_dense_feedback_bins(tmp_path) -> N assert event["detection_rate"] == 1.0 +def test_visibility_precheck_accepts_interpolated_endpoint_fraction(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 1 + ) + item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) + # Field regression: the motor reached zero with the physical 2-u8 + # deadband, while the last image-timestamped feedback was 2.502 u8. + commands = [2.5021463960247776, *range(3, 255)] + frames = [] + for command in commands: + state = [255.0] * 20 + state[1] = float(command) + frames.append(_frame("side", state)) + transitions: list[str] = [] + retries: list[str] = [] + node = SimpleNamespace( + active_sweep=item, + sweep_frames=frames, + synchronised_endpoint_tolerance_margin_u8=1.0, + minimum_sweep_bins=32, + maximum_bin_gap=16, + minimum_detection_rate=0.95, + views={"side": SimpleNamespace(valid_rate=1.0)}, + sweep_detection_total_frames=500, + sweep_detection_valid_frames=382, + raw_path=tmp_path / "raw_samples.jsonl", + active_sweep_is_fit_retry=False, + sweep_index=0, + sweep_items=[item], + profile=RIGHT_19_HAND_PROFILE, + normal_calibration_speed=15, + index_roll_calibration_speed=5, + index_flex_calibration_speed=10, + precheck_speed_metrics={}, + formal_speed_scales={}, + sweep_retry_counts={}, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + _retry_active_sweep_or_pause=lambda reason: retries.append(reason), + _begin_return_baseline=lambda after: transitions.append(after), + ) + + G20ThreeCameraCalibrationNode._finish_active_sweep(node) + + assert retries == [] + assert transitions == ["fit"] + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["detection_rate"] == 0.764 + assert event["detection_rate_below_threshold_views"] == ["side"] + assert event["trajectory_coverage_override_views"] == ["side"] + + +def test_low_precheck_rate_still_retries_when_trajectory_is_sparse(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 1 + ) + item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) + frames = [] + for command in (0, 127, 255): + state = [255.0] * 20 + state[1] = float(command) + frames.append(_frame("side", state)) + retries: list[str] = [] + node = SimpleNamespace( + active_sweep=item, + sweep_frames=frames, + minimum_sweep_bins=32, + maximum_bin_gap=16, + minimum_detection_rate=0.95, + sweep_detection_total_frames=100, + sweep_detection_valid_frames=3, + profile=RIGHT_19_HAND_PROFILE, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + _retry_active_sweep_or_pause=lambda reason: retries.append(reason), + ) + + G20ThreeCameraCalibrationNode._finish_active_sweep(node) + + assert retries == ["task_precheck_detection_rate_too_low:side"] + + +def test_visibility_precheck_rejects_sample_outside_sync_endpoint_margin() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 1 + ) + item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) + frames = [] + for command in [3.01, *range(4, 255)]: + state = [255.0] * 20 + state[1] = float(command) + frames.append(_frame("side", state)) + retries: list[str] = [] + node = SimpleNamespace( + active_sweep=item, + sweep_frames=frames, + synchronised_endpoint_tolerance_margin_u8=1.0, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + _retry_active_sweep_or_pause=lambda reason: retries.append(reason), + ) + + G20ThreeCameraCalibrationNode._finish_active_sweep(node) + + assert retries == ["sweep_missing_endpoint_bin"] + + +def test_synchronised_endpoint_margin_does_not_change_motion_deadband() -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) + node = SimpleNamespace( + synchronised_endpoint_tolerance_margin_u8=1.0, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, + ) + + assert node._endpoint_tolerance_for_spec(spec, 0) == 2.0 + assert ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + node, spec, 0 + ) + == 3.0 + ) + + def test_formal_sweep_still_rejects_17_u8_feedback_gap(tmp_path) -> None: spec = next( item @@ -2235,6 +3078,62 @@ def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None: assert pauses == ["return_baseline_timeout"] +def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "ring_pitch_side" + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + runtime = SimpleNamespace(pnp_invalid_since=10.0, pnp_reset_count=0) + resets: list[tuple[object, bool]] = [] + diagnostic_resets: list[object] = [] + commands: list[list[int]] = [] + start_frames = [object()] + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + active_sweep=item, + active_sweep_is_fit_retry=False, + views={"side": runtime}, + sweep_start_frames=start_frames, + motion_retry_counts={}, + automatic_motion_retry_limit=2, + raw_path=tmp_path / "raw_samples.jsonl", + state=STATE_PREPARE_SWEEP, + position_hold_since=1.0, + reason="", + preparation_command_u8=tuple([255] * 20), + baseline_command=tuple([255] * 20), + latest_state_u8=tuple([255] * 20), + _reset_view_trackers=lambda selected, preserve_task_reference=False: ( + resets.append((selected, preserve_task_reference)) + ), + _reset_view_pnp_diagnostics=lambda selected: ( + diagnostic_resets.append(selected) + ), + _command_vector_error_u8=lambda command: 0.0, + _normal_speed_profile=lambda: [15] * 5, + _publish_speed_profile=lambda profile: None, + _publish_command=lambda command: commands.append(command), + _reset_motion_progress=lambda now, error: None, + _pause=lambda reason: pytest.fail(f"unexpected pause: {reason}"), + ) + + G20ThreeCameraCalibrationNode._retry_motion_or_pause( + node, "sweep_start_tag_timeout", 20.0 + ) + + assert resets == [(runtime, True)] + assert diagnostic_resets == [runtime] + assert runtime.pnp_invalid_since is None + assert runtime.pnp_reset_count == 1 + assert start_frames == [] + assert len(commands) == 1 + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["pnp_trackers_reset"] == ["side"] + assert event["task_reference_preserved"] is True + + def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: pauses: list[str] = [] node = SimpleNamespace( @@ -2349,6 +3248,71 @@ def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None: ) == {0} +def test_axis_line_spread_localizes_one_outlier_for_four_rounds() -> None: + axes = [ + [-0.043565, 0.036698, -0.998376], + [-0.043462, 0.047539, -0.997923], + [-0.045335, 0.041627, -0.998104], + [-0.043689, 0.044394, -0.998058], + ] + points_mm = [ + [-43.981, 52.532, 1101.515], + [-43.999, 50.684, 1101.530], + [-44.080, 49.366, 1101.310], + [-44.252, 50.672, 1101.511], + ] + measurements = [ + SimpleNamespace( + axis_common_xyz=axis, + point_common_xyz_m=[0.001 * value for value in point], + ) + for axis, point in zip(axes, points_mm) + ] + + assert _isolated_axis_line_cycle_outliers( + measurements, 0.001 + ) == {0} + + +def test_axis_line_spread_does_not_localize_ambiguous_drift() -> None: + measurements = [ + SimpleNamespace( + axis_common_xyz=[0.0, 0.0, 1.0], + point_common_xyz_m=[0.0, offset_m, 0.0], + ) + for offset_m in (0.0, 0.001, 0.002, 0.003) + ] + + assert not _isolated_axis_line_cycle_outliers( + measurements, 0.001 + ) + + +def test_fit_retry_preserves_first_cycle_pnp_task_reference() -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) + + assert _preserve_pnp_task_reference_for_sweep( + SweepItem(spec, 0, DIRECTION_DECREASING), + is_fit_retry=True, + has_precheck_anchor=False, + ) + assert _preserve_pnp_task_reference_for_sweep( + SweepItem(spec, 0, DIRECTION_DECREASING), + is_fit_retry=False, + has_precheck_anchor=True, + ) + assert not _preserve_pnp_task_reference_for_sweep( + SweepItem(spec, 0, DIRECTION_DECREASING), + is_fit_retry=False, + has_precheck_anchor=False, + ) + assert _preserve_pnp_task_reference_for_sweep( + SweepItem(spec, 1, DIRECTION_DECREASING), + is_fit_retry=False, + has_precheck_anchor=False, + ) + + def test_provisional_axis_spread_marks_only_the_isolated_round() -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) records = _image_cycle_records([math.radians(47.0)] * 4) @@ -2660,22 +3624,147 @@ def test_cross_view_roll_axis_disagreement_skips_fusion(tmp_path) -> None: # bias in session 20260820_105535: it must keep the trusted front axis # and record a diagnostic instead of failing the joint. assert measurement.axis_direction_source != "cross_view_weighted_fusion" + assert measurement.pose_axis_line_source_joints == ( + "pinky_mcp_roll_side", + ) diagnostics = [ json.loads(line) for line in node.raw_path.read_text().splitlines() ] assert diagnostics[-1]["kind"] == "cross_view_roll_axis_diagnostic" - assert diagnostics[-1]["decision"] == "skip_fusion_use_primary" + assert diagnostics[-1]["decision"] == ( + "diagnostic_only_pose_disagreement_use_primary" + ) assert 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 +def test_cross_view_axis_group_uses_one_physical_radius(tmp_path) -> None: + node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) + measurements = [ + node._fit_axis_measurement("pinky_mcp_roll", cycle) + for cycle in range(node.repetitions) + ] + + refined = G20ThreeCameraCalibrationNode._refit_cross_view_axis_line_group( + node, "pinky_mcp_roll", measurements + ) + + assert all( + measurement.axis_point_source + == "side_circle_shared_radius_cross_view" + for measurement in refined + ) + assert all( + cross_view_side_line_source(measurement) + == "pinky_mcp_roll_side" + for measurement in refined + ) + assert axis_line_cycle_rms_m(refined) < 1.0e-6 + + +def test_cross_view_side_line_failure_rescans_only_side_alias( + tmp_path, monkeypatch +) -> None: + node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) + original_fit_axis = node._fit_axis_measurement + + def displaced_side_line(name, cycle): + measurement = original_fit_axis(name, cycle) + if name == "pinky_mcp_roll" and cycle == node.repetitions - 1: + point = np.asarray(measurement.point_common_xyz_m, dtype=float) + point[0] += 0.0024 + return replace( + measurement, + point_common_xyz_m=tuple(float(value) for value in point), + ) + return measurement + + node._fit_axis_measurement = displaced_side_line + # This test isolates the downstream repeatability gate. The production + # shared-radius refit legitimately removes an artificial point-only + # displacement that is absent from the underlying trajectory records. + monkeypatch.setattr( + G20ThreeCameraCalibrationNode, + "_refit_cross_view_axis_line_group", + lambda _self, _name, measurements: list(measurements), + ) + monkeypatch.setitem( + G20ThreeCameraCalibrationNode._provisional_fit_failures.__globals__, + "_isolated_axis_line_cycle_outliers", + lambda measurements, limit_m: {3}, + ) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + line_failures = [ + failure + for failure in failures + if failure["metric"] == "axis_line_cycle_rms_mm" + ] + + assert line_failures + assert { + failure["joint"] for failure in line_failures + } == {"pinky_mcp_roll_side"} + assert all( + failure["model_joint"] == "pinky_mcp_roll" + and failure["quality_source_joints"] == ["pinky_mcp_roll_side"] + for failure in line_failures + ) + assert all(failure["cycle"] == 4 for failure in line_failures) + assert all( + failure["inlier_cycles"] == [1, 2, 3] + for failure in line_failures + ) + assert _fit_retry_joint_names( + RIGHT_19_HAND_PROFILE, spec, line_failures + ) == {"pinky_mcp_roll_side"} + + +def test_cross_view_side_pose_residual_uses_line_repeatability_gate( + tmp_path, +) -> None: + node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) + original_fit_axis = node._fit_axis_measurement + + def biased_but_repeatable_side_pose(name, cycle): + measurement = original_fit_axis(name, cycle) + if name == "pinky_mcp_roll": + return replace(measurement, pose_axis_line_rms_m=0.00125) + return measurement + + node._fit_axis_measurement = biased_but_repeatable_side_pose + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + assert not [ + failure + for failure in failures + if failure["metric"] + in {"axis_pose_line_rms_mm", "axis_line_cycle_rms_mm"} + ] + + def test_cross_view_roll_systematic_line_offset_skips_fusion(tmp_path) -> None: - # Session 20260820_132727: the front roll-link and side PIP-link axis - # lines sit ~21 mm apart systematically while the axes differ by ~11.4 - # deg; both are inside the gross bounds, so the joint must keep the - # front-only axis instead of failing. + # Ring session 20260824_155323: the front screw-driven pseudo-line and + # side PIP-link physical line sit 36.6-38.2 mm apart while both views are + # individually clean. This finger-dependent offset must remain a + # diagnostic and must not reject the trusted-front/side-line fallback. node = _cross_view_roll_node( - tmp_path, side_tilt_deg=11.0, side_offset_m=[0.021, 0.0, 0.0] + tmp_path, side_tilt_deg=11.0, side_offset_m=[0.038, 0.0, 0.0] ) measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) @@ -2686,7 +3775,7 @@ def test_cross_view_roll_systematic_line_offset_skips_fusion(tmp_path) -> None: for line in node.raw_path.read_text().splitlines() ] assert diagnostics[-1]["kind"] == "cross_view_roll_axis_diagnostic" - assert 18.0 < diagnostics[-1]["line_distance_mm"] < 25.0 + assert 34.0 < diagnostics[-1]["line_distance_mm"] < 42.0 assert 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 @@ -2705,28 +3794,178 @@ def test_cross_view_roll_diagnostic_uses_real_node_logger(tmp_path) -> None: assert any("disagree" in message for message in warnings) -def test_cross_view_roll_axis_gross_disagreement_still_fails(tmp_path) -> None: +def test_cross_view_roll_axis_gross_disagreement_is_diagnostic_only( + tmp_path, +) -> None: node = _cross_view_roll_node(tmp_path, side_tilt_deg=20.0) - with pytest.raises( - ValueError, match="cross_view_roll_axis_gross_disagreement" - ): - node._fit_axis_measurement("pinky_mcp_roll", 0) + measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) + + assert measurement.axis_direction_source == "rotation" + diagnostics = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert diagnostics[-1]["decision"] == ( + "diagnostic_only_gross_pose_disagreement_use_primary" + ) -def test_cross_view_roll_axis_agreement_still_fuses(tmp_path) -> None: +def test_cross_view_roll_gross_disagreement_does_not_rescan(tmp_path) -> None: + node = _cross_view_roll_node(tmp_path, side_tilt_deg=20.0) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + gross_failures = [ + failure + for failure in failures + if str(failure.get("reason", "")).startswith( + "cross_view_roll_axis_gross_disagreement:" + ) + ] + + assert not gross_failures + diagnostics = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert any( + row.get("decision") + == "diagnostic_only_gross_pose_disagreement_use_primary" + for row in diagnostics + ) + + +def test_cross_view_roll_axis_agreement_keeps_published_primary(tmp_path) -> None: node = _cross_view_roll_node(tmp_path, side_tilt_deg=0.2) measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - assert measurement.axis_direction_source == "cross_view_weighted_fusion" + assert measurement.axis_direction_source == "rotation" + assert measurement.pose_axis_line_source_joints == ( + "pinky_mcp_roll_side", + ) -def test_side_alias_branch_gap_range_uses_relaxed_limit( +def test_cross_view_roll_side_pose_never_replaces_primary_direction( + tmp_path, monkeypatch +) -> None: + # Regression for session 20260824_171302. Three side-view cone + # residuals were just inside 5 deg and one was 5.037 deg. Selecting the + # camera per cycle mixed three side axes with one front axis and created a + # false 3.24 deg cycle spread although each camera was internally stable. + names = ( + "middle_mcp_roll", + "middle_mcp_roll_side", + "middle_mcp_pitch", + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + zero_profile=get_zero_calibration_profile( + "right", "g20_right_19" + ), + repetitions=4, + records_by_joint={ + name: [{"cycle": cycle} for cycle in range(4)] + for name in names + }, + source_urdf_path=tmp_path / "unused.urdf", + raw_path=tmp_path / "raw_samples.jsonl", + zero_maximum_axis_cycle_difference_rad=math.radians(0.75), + zero_maximum_axis_cone_mismatch_rad=math.radians(5.0), + axis_maximum_pose_line_rms_m=0.001, + cross_view_roll_maximum_axis_difference_rad=math.radians(15.0), + ) + primary_degrees = (8.0078, 8.1710, 8.0712, 8.0236) + secondary_degrees = (4.9043, 4.9136, 5.0374, 4.9389) + + def axis(degrees: float) -> tuple[float, float, float]: + angle = math.radians(degrees) + return (math.sin(angle), 0.0, math.cos(angle)) + + def measurement(name: str, cycle: int) -> JointAxisMeasurement: + if name == "middle_mcp_roll": + direction = axis(primary_degrees[cycle]) + elif name == "middle_mcp_roll_side": + direction = axis(secondary_degrees[cycle]) + else: + direction = (1.0, 0.0, 0.0) + return JointAxisMeasurement( + joint=name, + cycle=cycle, + axis_common_xyz=direction, + point_common_xyz_m=(0.0, 0.0, 0.0), + condition_state_u8=(255.0,) * 20, + plane_rms_m=0.0001, + radial_rms_m=0.0001, + rotation_circle_axis_difference_rad=0.0, + axis_direction_source="rotation", + pose_axis_line_rms_m=0.0001, + ) + + class Model: + def __init__(self, unused_path) -> None: + pass + + def axis_line(self, name, *, zero_offsets, joint_angles): + direction = ( + np.asarray([0.0, 0.0, 1.0]) + if name == "middle_mcp_roll" + else np.asarray([1.0, 0.0, 0.0]) + ) + return direction, np.zeros(3) + + monkeypatch.setattr( + G20ThreeCameraCalibrationNode, + "_fit_axis_measurement_raw", + lambda unused_self, name, cycle: measurement(name, cycle), + ) + monkeypatch.setitem( + G20ThreeCameraCalibrationNode._fit_axis_measurement.__globals__, + "UrdfKinematicModel", + Model, + ) + + measured = [ + G20ThreeCameraCalibrationNode._fit_axis_measurement( + node, "middle_mcp_roll", cycle + ) + for cycle in range(4) + ] + + assert { + item.axis_direction_source for item in measured + } == {"rotation"} + maximum_spread = max( + math.acos( + abs( + float( + np.clip( + np.asarray(left.axis_common_xyz) + @ np.asarray(right.axis_common_xyz), + -1.0, + 1.0, + ) + ) + ) + ) + for left in measured + for right in measured + ) + assert math.degrees(maximum_spread) < 0.75 + + +def test_side_alias_branch_gap_range_is_diagnostic_only( tmp_path, monkeypatch ) -> None: records = _image_cycle_records([math.radians(47.0)] * 3) - gaps_deg = [0.10, 0.46, 0.10] # range 0.36 deg, maximum 0.46 deg + gaps_deg = [0.10, 0.81, 0.10] # range 0.71 deg, maximum below 2 deg monkeypatch.setattr( "g20_thumb_apriltag_calibration.three_camera_node" ".baseline_hysteresis_by_cycle_rad", @@ -2760,6 +3999,19 @@ def test_side_alias_branch_gap_range_uses_relaxed_limit( for failure in alias_failures if failure["metric"] == "baseline_directional_gap_range_deg" ] + alias_diagnostics = [ + json.loads(line) + for line in (tmp_path / "raw_pinky_mcp_roll_side.jsonl") + .read_text() + .splitlines() + ] + assert any( + row.get("kind") == "validation_only_quality_diagnostic" + and row.get("metric") == "baseline_directional_gap_range_deg" + and row.get("actual") == 0.71 + and row.get("reference_limit") == 0.3 + for row in alias_diagnostics + ) canonical_failures = ( G20ThreeCameraCalibrationNode._provisional_fit_failures( @@ -2775,6 +4027,70 @@ def test_side_alias_branch_gap_range_uses_relaxed_limit( assert range_failure["limit"] == 0.3 +def test_side_alias_axis_cycle_spread_is_diagnostic_only(tmp_path) -> None: + records = _image_cycle_records([math.radians(47.0)] * 4) + multiview = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + name = "pinky_mcp_roll_side" + node = _fit_check_node({name: records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.repetitions = 4 + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {name: records} + node.raw_path = tmp_path / "raw_axis_alias.jsonl" + wrapped = node._fit_axis_measurement + axes = [ + np.asarray([0.0, 0.0, 1.0]), + np.asarray( + [ + 0.0, + math.sin(math.radians(1.1)), + math.cos(math.radians(1.1)), + ] + ), + np.asarray([0.0, 0.0, 1.0]), + np.asarray([0.0, 0.0, 1.0]), + ] + + def spread_axis(joint_name, cycle): + return replace( + wrapped(joint_name, cycle), + axis_common_xyz=axes[cycle], + radial_rms_m=0.0001, + pose_axis_line_rms_m=0.0001, + plane_rms_m=0.0001, + rotation_circle_axis_difference_rad=0.0, + ) + + node._fit_axis_measurement = spread_axis + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, replace(multiview, joints=(name,)), include_view_validity=False + ) + + assert not [ + failure + for failure in failures + if failure["metric"] == "axis_cycle_difference_deg" + ] + diagnostics = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert any( + row.get("kind") == "validation_only_quality_diagnostic" + and row.get("metric") == "axis_cycle_difference_deg" + and row.get("actual") == 1.1 + and row.get("reference_limit") == 0.75 + for row in diagnostics + ) + + def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None: records = _image_cycle_records([math.radians(47.0)] * 3) multiview = next( @@ -3043,6 +4359,35 @@ def test_task_validity_gate_fails_on_bad_task_counters() -> None: assert validity_failure["task_total_frames"] == 1000 +def test_right_19_task_validity_is_diagnostic_after_complete_sweeps() -> None: + node, _legacy_spec = _task_validity_node( + task_valid=650, task_total=1000, window=0.65 + ) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_19" + ) + node.records_by_joint["thumb_cmc_pitch"] = _image_cycle_records( + [math.radians(47.0)] * 3, + command_step=1, + ) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 0 + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert not [ + failure + for failure in failures + if failure["metric"] == "tag_valid_rate_percent" + ] + + def test_task_validity_gate_falls_back_to_window_without_counters() -> None: node, spec = _task_validity_node( task_valid=0, task_total=0, window=0.99 diff --git a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py index 02cf76a..d27c6cc 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -16,13 +16,18 @@ from g20_thumb_apriltag_calibration.urdf_zero import ( DIRECT_ZERO_JOINTS, INHERITED_ZERO_JOINTS, JointAxisMeasurement, + PalmOrientationMeasurement, UrdfKinematicModel, _angles_from_state, _zero_sensitive_axis_error_rad, + anchor_right_19_mechanical_endpoint_curves, baseline_hysteresis_by_cycle_rad, - derive_right_19_flexion_endpoint_offsets, + derive_right_19_mechanical_endpoint_offsets, fit_joint_axis_measurement, + fit_partial_palm_orientation_measurement, + fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, + select_cross_view_roll_direction_source, solve_urdf_zero_offsets, get_zero_calibration_profile, write_zero_corrected_urdf, @@ -49,6 +54,30 @@ RIGHT_SOURCE_URDF = REPOSITORY / ( ) +def test_cross_view_roll_direction_is_selected_once_by_group_consensus() -> None: + # Session 20260824_171302: the side residual in cycle 3 was 5.037 deg, + # just outside the 5 deg cone gate, while the other three side cycles + # passed and every front cycle was near 8 deg. Per-cycle selection mixed + # three side axes with one front axis and fabricated a 3.24 deg spread. + source = select_cross_view_roll_direction_source( + [math.radians(value) for value in (8.0078, 8.1710, 8.0712, 8.0236)], + [math.radians(value) for value in (4.9043, 4.9136, 5.0374, 4.9389)], + math.radians(5.0), + ) + + assert source == "secondary" + + +def test_cross_view_roll_direction_keeps_primary_without_consensus() -> None: + source = select_cross_view_roll_direction_source( + [math.radians(value) for value in (5.2, 4.8, 5.1, 4.9)], + [math.radians(value) for value in (4.7, 5.3, 4.8, 5.2)], + math.radians(5.0), + ) + + assert source == "primary" + + def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch(): parent = np.asarray([0.0, 0.0, 1.0]) predicted = np.asarray([1.0, 0.0, 0.0]) @@ -588,9 +617,12 @@ def _solve_synthetic_offsets( 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, + maximum_systematic_axis_cone_bias_degrees: float | None = None, pose_axis_line_rms_by_joint_m: dict[str, float] | None = None, joint_maximum_offset_degrees: dict[str, float] | None = None, validation_offset_bias_degrees: dict[str, float] | None = None, + base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8), + base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72), ): hand = get_hand_calibration_profile(side, layout_id) zero = get_zero_calibration_profile(side, layout_id) @@ -620,12 +652,23 @@ def _solve_synthetic_offsets( 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]) + base_rotation = Rotation.from_euler("xyz", base_euler_xyz_rad) + base_translation = np.asarray(base_translation_xyz_m) measurements: list[JointAxisMeasurement] = [] + palm_orientation_measurements: list[PalmOrientationMeasurement] = [] validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2 training_cycles = tuple(range(validation_cycle)) for cycle in range(validation_cycle + 1): + cycle_offsets = dict(offsets) + if cycle == validation_cycle: + cycle_offsets.update( + { + name: cycle_offsets[name] + math.radians(value) + for name, value in ( + validation_offset_bias_degrees or {} + ).items() + } + ) for joint in zero.axis_joints: state = list(baseline) if joint == "thumb_cmc_yaw": @@ -636,16 +679,6 @@ def _solve_synthetic_offsets( motor_by_joint=motor_by_joint, inherited_zero_joints=zero.inherited_zero_joints, ) - cycle_offsets = dict(offsets) - if cycle == validation_cycle: - cycle_offsets.update( - { - name: cycle_offsets[name] + math.radians(value) - for name, value in ( - validation_offset_bias_degrees or {} - ).items() - } - ) axis, point = model.axis_line( joint, zero_offsets=cycle_offsets, joint_angles=angles ) @@ -734,9 +767,36 @@ def _solve_synthetic_offsets( ).get(joint, 0.0), ) ) + for source_joint, model_joint in ( + hand.palm_orientation_sources or {} + ).items(): + state = list(baseline) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=zero.inherited_zero_joints, + ) + axis, _ = model.axis_line( + model_joint, + zero_offsets=cycle_offsets, + joint_angles=angles, + ) + palm_orientation_measurements.append( + PalmOrientationMeasurement( + source_joint=source_joint, + model_joint=model_joint, + cycle=cycle, + axis_common_xyz=tuple(base_rotation.apply(axis)), + condition_state_u8=tuple(state), + observed_arc_rad=math.radians(45.0), + rotation_orthogonal_rms_rad=math.radians(0.1), + ) + ) result = solve_urdf_zero_offsets( source_urdf=source, measurements=measurements, + palm_orientation_measurements=palm_orientation_measurements, curves=curves, motor_by_joint=motor_by_joint, hand_type=side, @@ -747,6 +807,13 @@ def _solve_synthetic_offsets( }, training_cycles=training_cycles, validation_cycle=validation_cycle, + maximum_systematic_axis_cone_bias_rad=( + None + if maximum_systematic_axis_cone_bias_degrees is None + else math.radians( + maximum_systematic_axis_cone_bias_degrees + ) + ), ) return zero, result @@ -788,9 +855,9 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp expected, abs=0.05 ) assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx( - 0.0, abs=0.05 + -1.5, abs=0.05 ) - assert zero.fixed_direct_zero_offsets_rad == {"thumb_mcp": 0.0} + assert zero.fixed_direct_zero_offsets_rad == {} assert result.training_cycles == (0, 1, 2) assert result.validation_cycle == 3 assert all( @@ -803,7 +870,203 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) -def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> None: +def _partial_orientation_records( + *, + tag_mount: Rotation, + common_rotation: Rotation, + maximum_angle_deg: float = 30.0, + tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015), +) -> list[dict[str, object]]: + axis_parent = np.asarray([0.0, 1.0, 0.0]) + parent_pose = { + "translation_xyz_m": [0.2, -0.1, 0.7], + "quaternion_xyzw": list(common_rotation.as_quat()), + } + records: list[dict[str, object]] = [] + commands = tuple(range(255, 174, -4)) + for direction, ordered in ( + ("decreasing", commands), + ("increasing", tuple(reversed(commands))), + ): + for command in ordered: + fraction = (255 - command) / (255 - commands[-1]) + angle = math.radians(maximum_angle_deg) * fraction + relative = ( + Rotation.from_rotvec(axis_parent * angle) * tag_mount + ) + relative_translation = Rotation.from_rotvec( + axis_parent * angle + ).apply(tag_offset_xyz_m) + state = [255.0] * 20 + state[1] = float(command) + records.append( + { + "cycle": 0, + "direction": direction, + "command_u8": command, + "relative_quaternion_xyzw": list(relative.as_quat()), + "relative_translation_xyz_m": list( + relative_translation + ), + "parent_pose_common": parent_pose, + "state_u8": state, + } + ) + return records + + +def test_partial_palm_direction_ignores_fixed_tag_mount_pose() -> None: + common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) + first = fit_partial_palm_orientation_measurement( + "index_mcp_pitch_front_axis", + "index_mcp_pitch", + _partial_orientation_records( + tag_mount=Rotation.from_euler("xyz", [0.1, 0.2, -0.4]), + common_rotation=common_rotation, + ), + cycle=0, + zero_command_u8=255, + ) + second = fit_partial_palm_orientation_measurement( + "index_mcp_pitch_front_axis", + "index_mcp_pitch", + _partial_orientation_records( + tag_mount=Rotation.from_euler("xyz", [-0.7, 0.45, 0.9]), + common_rotation=common_rotation, + tag_offset_xyz_m=(-0.035, 0.008, 0.041), + ), + cycle=0, + zero_command_u8=255, + ) + expected = common_rotation.apply([0.0, 1.0, 0.0]) + + assert abs(np.dot(first.axis_common_xyz, expected)) == pytest.approx( + 1.0, abs=1.0e-8 + ) + assert abs( + np.dot(first.axis_common_xyz, second.axis_common_xyz) + ) == pytest.approx(1.0, abs=1.0e-8) + + +def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None: + records = _partial_orientation_records( + tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), + common_rotation=Rotation.identity(), + ) + baseline = fit_partial_palm_orientation_measurement( + "index_mcp_pitch_front_axis", + "index_mcp_pitch", + records, + cycle=0, + zero_command_u8=255, + ) + source = next( + record + for record in records + if record["direction"] == "decreasing" + and record["command_u8"] == 223 + ) + biased = dict(source) + biased["relative_quaternion_xyzw"] = list( + ( + Rotation.from_rotvec([math.radians(2.0), 0.0, 0.0]) + * Rotation.from_quat(source["relative_quaternion_xyzw"]) + ).as_quat() + ) + fitted = fit_partial_palm_orientation_measurement( + "index_mcp_pitch_front_axis", + "index_mcp_pitch", + [*records, *([biased] * 200)], + cycle=0, + zero_command_u8=255, + ) + + difference = math.acos( + abs( + float( + np.clip( + np.dot(baseline.axis_common_xyz, fitted.axis_common_xyz), + -1.0, + 1.0, + ) + ) + ) + ) + assert difference < math.radians(0.25) + + +def test_partial_palm_direction_rejects_too_short_visible_arc() -> None: + records = _partial_orientation_records( + tag_mount=Rotation.identity(), + common_rotation=Rotation.identity(), + maximum_angle_deg=5.0, + ) + + with pytest.raises(ValueError, match="visible rotation arc"): + fit_partial_palm_orientation_measurement( + "index_mcp_pitch_front_axis", + "index_mcp_pitch", + records, + cycle=0, + zero_command_u8=255, + ) + + +def test_partial_palm_direction_uses_three_of_four_visible_sources() -> None: + sources = { + f"{finger}_mcp_pitch_front_axis": f"{finger}_mcp_pitch" + for finger in ("index", "middle", "ring", "pinky") + } + records = { + source: _partial_orientation_records( + tag_mount=Rotation.from_euler( + "xyz", [0.1 * index, -0.2, 0.3] + ), + common_rotation=Rotation.identity(), + maximum_angle_deg=(5.0 if index == 3 else 30.0), + ) + for index, source in enumerate(sources) + } + + fitted, rejected = fit_partial_palm_orientation_measurements( + sources=sources, + records_by_joint=records, + motor_by_source={source: index + 1 for index, source in enumerate(sources)}, + baseline_command_u8=(255,) * 20, + cycles=(0,), + minimum_sources=3, + ) + + assert len(fitted) == 3 + assert len(rejected) == 1 + assert next(iter(rejected)).startswith("pinky_mcp_pitch_front_axis") + + +def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None: + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, + ] + _, first = _solve_synthetic_offsets( + "right", injected, layout_id="g20_right_19" + ) + _, moved = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + base_euler_xyz_rad=(-0.25, 0.55, -0.35), + base_translation_xyz_m=(-0.12, 0.28, 0.91), + ) + + assert moved.direct_offsets_rad == pytest.approx( + first.direct_offsets_rad, abs=1.0e-8 + ) + + +def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None: hand = get_hand_calibration_profile("right", "g20_right_19") curves = { name: _synthetic_curve(255, math.radians(50.0)) @@ -816,16 +1079,35 @@ def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> No curves[f"{finger}_pip"] = _synthetic_curve( 255, math.radians(103.0) ) + curves["thumb_mcp"] = _synthetic_curve(255, math.radians(70.0)) + curves["thumb_cmc_roll"] = _synthetic_curve( + 255, math.radians(76.0) + ) + curves["thumb_cmc_yaw"] = _synthetic_curve( + 255, math.radians(91.0) + ) + curves["thumb_cmc_pitch"] = _synthetic_curve( + 255, math.radians(47.0) + ) - offsets = derive_right_19_flexion_endpoint_offsets( + offsets = derive_right_19_mechanical_endpoint_offsets( RIGHT_SOURCE_URDF, curves ) assert set(offsets) == { - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), } + assert math.degrees(offsets["thumb_mcp"]) == pytest.approx( + math.degrees(1.25) - 70.0 + ) + assert not { + "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch" + }.intersection(offsets) assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx( math.degrees(1.22) - 71.0 ) @@ -834,6 +1116,100 @@ def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> No ) +def _settled_endpoint_records( + travel_rad: float, + *, + increasing_travel_rad: float | None = None, +) -> list[dict[str, object]]: + fixed_mounting = Rotation.from_euler("xyz", [0.7, -0.4, 1.1]) + mounted_axis = Rotation.from_euler("xyz", [-0.3, 0.8, 0.2]).apply( + [0.0, 0.0, 1.0] + ) + result: list[dict[str, object]] = [] + for direction, branch_travel in ( + ("decreasing", travel_rad), + ( + "increasing", + travel_rad + if increasing_travel_rad is None + else increasing_travel_rad, + ), + ): + endpoint = fixed_mounting * Rotation.from_rotvec( + mounted_axis * branch_travel + ) + for command, rotation in ((0, endpoint), (255, fixed_mounting)): + result.append( + { + "direction": direction, + "requested_command_u8": command, + "relative_quaternion_xyzw": rotation.as_quat().tolist(), + } + ) + return result + + +def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None: + hand = get_hand_calibration_profile("right", "g20_right_19") + names = { + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), + } + projected_travel = math.radians(105.832) + direct_travel = math.radians(103.820) + curves = { + name: _synthetic_curve(255, projected_travel) + for name in hand.measured_joints + } + records = { + name: _settled_endpoint_records(direct_travel) for name in names + } + + anchored = anchor_right_19_mechanical_endpoint_curves(curves, records) + + assert curves["middle_pip"].angle_rad[0] == pytest.approx( + projected_travel + ) + for name in names: + assert anchored[name].angle_rad[0] == pytest.approx(direct_travel) + assert anchored[name].angle_rad[255] == pytest.approx(0.0) + assert anchored[name].circle[ + "mechanical_endpoint_direct_travel_rad" + ] == pytest.approx(direct_travel) + assert anchored["thumb_cmc_pitch"] is curves["thumb_cmc_pitch"] + + +def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None: + hand = get_hand_calibration_profile("right", "g20_right_19") + names = { + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), + } + curves = { + name: _synthetic_curve(255, math.radians(103.0)) + for name in hand.measured_joints + } + records = { + name: _settled_endpoint_records(math.radians(103.0)) + for name in names + } + records["middle_pip"] = _settled_endpoint_records( + math.radians(103.0), + increasing_travel_rad=math.radians(104.5), + ) + + with pytest.raises(ValueError, match="endpoint directions disagree"): + anchor_right_19_mechanical_endpoint_curves(curves, records) + + def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: # A shared electrical centre belongs to the four-motor common datum; the # strict 3 deg assembly guard applies to finger-to-finger deviations. @@ -919,6 +1295,19 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: validation_offset_bias_degrees={"thumb_cmc_roll": 2.0}, ) + finger_rolls = tuple( + name + for name in zero.direct_zero_joints + if name.endswith("_mcp_roll") and not name.startswith("thumb_") + ) + roll_common = float( + np.median( + [ + injected[zero.direct_zero_joints.index(name)] + for name in finger_rolls + ] + ) + ) assert math.degrees( result.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(injected[0], abs=0.05) @@ -1032,12 +1421,14 @@ def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> Non def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None: right = get_zero_calibration_profile("right") + right_19 = get_zero_calibration_profile("right", "g20_right_19") 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 + assert "thumb_mcp" not in right_19.fixed_direct_zero_offsets_rad def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None: @@ -1091,18 +1482,24 @@ def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None: 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] +def test_thumb_mcp_is_not_hard_coded_to_original_cad_zero() -> None: + offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0, *([0.0] * 9)] _, result = _solve_synthetic_offsets( "right", offsets, + layout_id="g20_right_19", 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)) - assert "thumb_ip" not in result.validation_error_by_joint_rad + assert math.degrees(result.direct_offsets_rad["thumb_mcp"]) == pytest.approx( + -40.0, abs=0.05 + ) + assert all( + math.degrees(value) == pytest.approx(-40.0, abs=0.05) + for value in result.cycle_offsets_rad["thumb_mcp"] + ) + assert result.validation_error_by_joint_rad["thumb_ip"] < math.radians(0.05) 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"): @@ -1149,6 +1546,66 @@ def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None ) +def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None: + injected = [0.0] * 16 + _, result = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + inject_observer_cone_bias_degrees=8.0, + maximum_systematic_axis_cone_bias_degrees=15.0, + ) + + assert result.passed is True + assert math.degrees( + result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"] + ) == pytest.approx(8.0) + assert result.axis_cone_bias_classification_by_joint[ + "thumb_cmc_yaw" + ] == "stable_cross_view_or_planar_pnp_bias" + + +def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None: + injected = [0.0] * 16 + _, result = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + inject_observer_cone_bias_degrees=16.0, + maximum_systematic_axis_cone_bias_degrees=15.0, + ) + + assert result.passed is False + assert result.failure_reasons["thumb_cmc_yaw"] == ( + "zero_axis_cone_mismatch_too_large" + ) + + +def test_right_19_finger_roll_common_gauge_is_removed_before_limits() -> None: + zero = get_zero_calibration_profile("right", "g20_right_19") + injected = [0.0] * len(zero.direct_zero_joints) + deviations = { + "index_mcp_roll": -0.4, + "middle_mcp_roll": 0.7, + "ring_mcp_roll": 0.1, + "pinky_mcp_roll": -0.1, + } + for name, deviation in deviations.items(): + injected[zero.direct_zero_joints.index(name)] = 18.0 + deviation + + _, result = _solve_synthetic_offsets( + "right", injected, layout_id="g20_right_19" + ) + + assert result.passed is True + common = float(np.median(list(deviations.values()))) + for name, deviation in deviations.items(): + expected = deviation - common + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + expected, abs=0.05 + ) + + def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None: _, result = _solve_synthetic_offsets( "right",