From ef656812305e32a104935b0d499ed0b537af3965 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Fri, 21 Aug 2026 12:21:39 +0800 Subject: [PATCH] =?UTF-8?q?G20=E5=9B=9B=E6=8C=87=E5=8D=95=E7=8B=AC?= =?UTF-8?q?=E6=A0=87=E5=AE=9A=EF=BC=88=E5=B0=91=E6=9C=AB=E7=AB=AFtag?= =?UTF-8?q?=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 4 +- src/g20_thumb_apriltag_calibration/README.md | 157 +- .../config/g20_right_product.yaml | 36 + .../config/three_camera_calibration.yaml | 77 +- .../three_camera_tags_g20_right_15.yaml | 62 + .../calibrated_joint_state_bridge.py | 99 +- .../full_hand.py | 1137 ++- .../offline_replay.py | 856 ++- .../one_command.py | 553 ++ .../operator_report.py | 394 ++ .../g20_thumb_apriltag_calibration/pnp.py | 350 +- .../g20_thumb_apriltag_calibration/product.py | 237 + .../publication.py | 569 ++ .../three_camera_diagnostics.py | 207 +- .../three_camera_node.py | 6123 ++++++++++++++++- .../trajectory.py | 13 +- .../urdf_zero.py | 1136 ++- .../calibrated_joint_state_bridge.launch.py | 2 +- .../launch/three_camera_calibration.launch.py | 70 +- .../package.xml | 2 +- src/g20_thumb_apriltag_calibration/setup.py | 6 +- .../test_calibrated_joint_state_bridge.py | 90 + .../test/test_config.py | 69 +- .../test/test_full_hand.py | 463 ++ .../test/test_g20_right_product.py | 1449 ++++ .../test/test_pnp.py | 173 + .../test/test_three_camera_diagnostics.py | 122 +- .../test/test_three_camera_retry.py | 2401 ++++++- .../test/test_urdf_zero.py | 341 +- 29 files changed, 16589 insertions(+), 609 deletions(-) create mode 100644 src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml create mode 100644 src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py diff --git a/.gitignore b/.gitignore index 11b5c74..4d6c492 100644 --- a/.gitignore +++ b/.gitignore @@ -62,7 +62,7 @@ Thumbs.db # src/linkerhand_retarget/resource/linkerforce_v2/profiles/. /profiles/ /calibration_output/ -/config/g20_three_camera_extrinsics.yaml +/config/*_three_camera_extrinsics.yaml *.wear_check.json *.checkpoint.json *.verification.json @@ -92,3 +92,5 @@ candump-* # Local Codex/agent workspace metadata /.agents/ /.codex/ +/.codebuddy/ +/.zcode/ diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 324e28c..3d040e7 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -1,5 +1,136 @@ # G20 左右手 AprilTag 标定 +## G20右手正式一键标定 + +固定三相机和15张Tag安装完成后，用户只运行： + +```bash +ros2 run g20_thumb_apriltag_calibration calibrate_g20_right +``` + +开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经 +完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的 +独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 +最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据 +当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 +失败、把会话拉回靠前的关节。需要强制从第一个关节 +重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为 +一个完整同步任务断点；只有两边数据都完整时才复用。 + +命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 +11个可求解主动关节URDF零位修正（`thumb_mcp`固定CAD零位）、 +17条实测命令曲线以及4条 +源URDF mimic派生DIP曲线发布。 +终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。 + +正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在 +JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证 +全部通过后更新。 + +## G20右手15-Tag底层调试入口 + +以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简 +schema v4 JSON，不再生成schema v5运行文件。 + +新布局用独立参数启用，原有左右手11-Tag流程仍默认使用 +`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖： + +```bash +ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \ + hand_type:=right \ + tag_layout:=g20_right_15 \ + serial_number:=G20_RIGHT_001 \ + camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ + source_urdf_expected_sha256:= +``` + +15张Tag均为16 mm `tag36h11`。正面为 +`0,1,2,3,10,11,12,13`，侧面为 +`4,5,6,15,17`，上面为`8,9`；ID `7,14,16,18`不再粘贴。详细角色以 +`config/three_camera_tags_g20_right_15.yaml`为唯一软件配置源。程序执行4项拇指任务， +以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、 +侧面PIP，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过 +各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic +multiplier生成，不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检 +0/127/255可见性； + +基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的 +固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中 +允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不 +放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的 +实时Tag。锁定后不得移动相机、手掌底座或整只手，否则缓存参考失效，必须重新启动 +标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计； +任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把 +采集质量良好的任务误判为可见性失败。 + +取消末端Tag后，四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准； +因此PIP动态曲线仍为视觉实测，但PIP的URDF静态零位保留源CAD，不猜测写入。 +正式修正范围为拇指4个主动关节，以及四指各自的MCP roll/MCP pitch，共12个。 +侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出； +同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时 +运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” +等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。 +`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。 +同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出， +不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且 +当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机 +退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的 +"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺 +不超过8，会把非roll任务四轮正式速度最多提高到预检速度的1.5倍；否则保持原保守速度。 +四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理 +零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、 +四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。 +预检、正式四轮和拟合重扫始终使用速度5。 +正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈； +方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再 +替代静态保持姿态。 +前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。 +每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化，但同一任务第1轮 +已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的 +IPPE镜像解。baseline标准接近和全部质量门限保持不变。 +电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中 +排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放 +`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。 + +当前15-Tag产品流程发布精简schema v4：21条运行时曲线中，17条来自当前会话的 +视觉实测，四指DIP按源URDF受保护的mimic multiplier由同指PIP曲线派生。 +URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch`，共12个， +其中`thumb_mcp`字段固定为0而不会修改源URDF； +四指PIP、`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的系统性位置差（丝杠平移连杆），属预期现象。侧视校验通道 +（`*_mcp_roll_side`）的 +分支间隙跨轮极差上限放宽为 +`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°（绝对间隙1.5°上限 +不变），其`axis_pose_line_rms`降为诊断，不再作为准入门限；径向、平面、 +圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受 +姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限； +径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、 +遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。 +8个组合姿态仅保留为开发诊断，正式产品默认不执行。15-Tag布局的侧面每根手指只有一张 +PIP Tag，并不存在可独立验证的DIP Tag；同时轴线零位求解不提供适合绝对笛卡尔位置验收的 +手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 +`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。 +现场需要区分某根手指的roll机构回差与单机位误差时，可设置 +`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次 +正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务， +双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果 +一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久 +锁定URDF发布，不能用`resume`转换成正式标定。 +schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅 +`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动 +方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。 +schema v4继续兼容旧 +命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。 + ## 三机位三维关节轴零位标定（schema v4） 正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有 @@ -177,7 +308,7 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn 前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位 差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的 -95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。 +训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。 单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动 耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差， @@ -209,18 +340,22 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn 生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的 `origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终 -JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。 +JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。 坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、 重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%， 端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时 连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并 暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫 -完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终 -拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序 +完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色 +预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算 +（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到 +后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前 +关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序 只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复 失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试 -仍保存在 `raw_samples.jsonl`。 +仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的 +PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。 每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。 程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组 @@ -273,20 +408,23 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试 ```text calibration_output/G20_LEFT_001/<时间戳>/ g20_left_G20_LEFT_001_calibration.json -src/.../g20_left/ linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf + meshes/*.STL calibration_output/G20_RIGHT_001/<时间戳>/ g20_right_G20_RIGHT_001_calibration.json -src/.../g20_right/ linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf + meshes/*.STL ``` 文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、 `zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从 指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或 覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能 -改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、 +改变；当前15-Tag右手保留12个主动静态零位字段，其中11个由数据求解，`thumb_mcp` +固定为原始CAD零位0；四指PIP虽保留字段但数值固定为0。 +未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到 +同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。 完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机 @@ -315,7 +453,8 @@ ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch. calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json ``` -默认订阅 `/cb_right_hand_control_cmd`，发布 +schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅 +`/g20/cb_right_hand_control_cmd`。两者均发布 `/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个 发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令， 因此不会静默退回旧标定。 diff --git a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml new file mode 100644 index 0000000..6711ca9 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml @@ -0,0 +1,36 @@ +schema_version: 1 +model: G20 +side: right +serial_number: G20_RIGHT_001 +can_interface: can0 +output_root: calibration_output + +cameras: + front: + serial_number: DB2163742 + camera_name: hikrobot_front_DB2163742 + camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + side: + serial_number: DB2163749 + camera_name: hikrobot_side_DB2163749 + camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + top: + serial_number: DB2163739 + camera_name: hikrobot_top_DB2163739 + camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml + +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 + calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml + calibration_config_sha256: 80758d0a240a7dea1426d4fad688b09eac8360560a1d5870d709b7bb103d3280 + tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml + tag_config_sha256: 7ce50be80dfccb3529c34d5bd601090b9d378e2bf6abcef2d5b93dc6a4da99fd + +release: + # Each task already contains three training cycles plus an isolated fourth + # holdout, so a second complete hardware session duplicates hours of motion. + required_independent_passes: 1 + static_repeatability_deg: 1.0 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 dea3733..fa1d1d6 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -16,14 +16,24 @@ g20_calibration: normal_calibration_speed: 15 index_roll_calibration_speed: 5 index_flex_calibration_speed: 10 + # 15-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量， + # 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束， + # 始终保持速度5；任一方向采样余量不足也保持原速度。 + 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 speed_setting_settle_seconds: 0.25 # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。 tag_size_m: 0.016 repetitions: 3 + # 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。 + g20_right_19_repetitions: 4 preflight_frames: 60 minimum_detection_rate: 0.95 minimum_detection_hz: 15.0 + minimum_feedback_hz: 25.0 maximum_hamming: 0 minimum_decision_margin: 30.0 minimum_edge_pixels: 30.0 @@ -36,9 +46,15 @@ g20_calibration: pnp_tracker_reset_seconds: 5.0 # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。 pnp_group_initialization_frames: 8 - # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。 + # 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。 pnp_group_normal_alignment_scale_deg: 5.0 pnp_group_maximum_normal_alignment_deg: 15.0 + # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic + # 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、 + # 缩放或替代被动IP的自身Tag实测曲线。 + thumb_ip_pnp_coupling_multiplier: 1.03 + thumb_ip_pnp_coupling_scale_deg: 3.0 + thumb_ip_pnp_maximum_coupling_residual_deg: 7.5 top_pnp_invalid_reset_seconds: 1.0 # 三维位姿必须与实测20通道状态严格按时间戳配对。 maximum_state_image_skew_ms: 50.0 @@ -54,17 +70,22 @@ g20_calibration: # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。 axis_maximum_rotation_circle_difference_deg: 1.0 # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合 - # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。 + # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。 active_maximum_rotation_orthogonal_rms_deg: 2.5 passive_maximum_rotation_orthogonal_rms_deg: 7.5 zero_maximum_axis_cycle_difference_deg: 0.75 # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。 zero_maximum_axis_cone_mismatch_deg: 5.0 + zero_maximum_observability_condition_number: 10000000000.0 zero_maximum_offset_deg: 20.0 - # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。 + # 四指MCP roll/pitch绝对静态零偏的保护范围；PIP仍保留CAD静态零位。 zero_finger_maximum_offset_deg: 3.0 endpoint_tolerance_u8: 2.0 + # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈 + # 必须已经稳定；大残差仍由机械卡滞保护处理。 + steady_checkpoint_command_feedback_tolerance_u8: 8.0 + steady_checkpoint_maximum_feedback_range_u8: 2.0 # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。 thumb_yaw_zero_endpoint_tolerance_u8: 4.0 # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。 @@ -72,12 +93,18 @@ g20_calibration: # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。 pinky_pip_zero_endpoint_tolerance_u8: 5.0 endpoint_hold_seconds: 0.5 + # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。 baseline_hold_seconds: 0.5 + minimum_baseline_hold_frames: 10 + # 15-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。 + task_precheck_hold_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 - # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停； + # 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停； # 这类故障不进入遮挡/超时的三次自动重扫。 - motor_stall_timeout_seconds: 8.0 + # 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。 + motor_stall_timeout_seconds: 2.0 + motor_stall_startup_grace_seconds: 1.0 motor_stall_minimum_progress_u8: 1.0 invalid_timeout_seconds: 3.0 minimum_sweep_frames: 40 @@ -85,15 +112,18 @@ g20_calibration: minimum_sweep_bins: 32 maximum_bin_gap: 16 # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。 - automatic_sweep_retry_limit: 3 + automatic_sweep_retry_limit: 2 # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。 automatic_fit_retry_limit: 2 automatic_motion_retry_limit: 2 + # 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll， + # 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。 + cross_view_roll_diagnostic_finger: "" # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。 provisional_warning_ratio: 1.25 retry_minimum_speed: 3 - retry_speed_scales: [0.8, 0.6, 0.5] - retry_endpoint_hold_seconds: [0.75, 1.0, 1.25] + retry_speed_scales: [0.8, 0.6] + retry_endpoint_hold_seconds: [0.75, 1.0] trajectory_maximum_plane_rms_m: 0.004 trajectory_maximum_radial_rms_m: 0.004 @@ -106,15 +136,40 @@ g20_calibration: trajectory_maximum_cycle_travel_difference_deg: 3.0 passive_maximum_cycle_travel_difference_deg: 10.0 maximum_monotonic_correction_deg: 2.0 - maximum_hysteresis_deg: 5.0 + # 旧布局仍用连续扫描正反程差门限；15-Tag产品的连续运动包含速度相关滞后， + # 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面 + # 的九点稳态command_maximum_direction_gap_deg。 + maximum_hysteresis_deg: 2.0 + # 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差 + # 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支 + # 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。 + baseline_maximum_hysteresis_deg: 0.5 + 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 passive_maximum_monotonic_correction_deg: 3.0 - passive_maximum_hysteresis_deg: 7.5 + passive_maximum_hysteresis_deg: 2.0 + # 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域 + # 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。 + command_maximum_direction_gap_deg: 2.0 - # 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。 + # 默认无额外随机动作；15-Tag产品最终一轮始终作为不可关闭的留出验证。 validation_enabled: false + # 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见 + # 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。 + # Developer diagnostic only. The 15-Tag product has one side PIP Tag per + # finger and no independent DIP Tag, while the formal fourth sweep cycle + # already provides an isolated per-joint holdout. + combination_validation_enabled: false + combination_validation_frames: 10 + combination_maximum_position_p95_m: 0.003 + combination_maximum_orientation_p95_deg: 2.0 validation_command_count: 3 validation_frames: 10 validation_seed: 20260804 validation_timeout_seconds: 20.0 maximum_validation_mae_deg: 1.0 maximum_validation_p95_deg: 2.0 + # 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。 + maximum_validation_error_deg: 3.0 + zero_maximum_confidence_half_width_deg: 1.5 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml new file mode 100644 index 0000000..81ff4a0 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml @@ -0,0 +1,62 @@ +/g20_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2, 3, 10, 11, 12, 13] + frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + +/g20_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [4, 5, 6, 15, 17] + frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016] + +/g20_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [8, 9] + frames: [top_base, thumb_yaw] + sizes: [0.016, 0.016] diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py index f227b07..1347746 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py @@ -1,8 +1,13 @@ -"""Map G20 u8 commands to URDF joint angles using one calibration JSON. +"""Map G20 u8 feedback to URDF joint angles using one calibration JSON. The static encoder-zero corrections in ``zero_angles`` are already baked into the corrected URDF joint origins. This bridge therefore publishes only the dynamic ``angle_rad`` values and never adds the static offsets a second time. + +Schema-v5 trajectories are fitted against timestamp-synchronised hardware +feedback, not controller set-points. They must therefore be queried with the +SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for +backwards compatibility only. """ from __future__ import annotations @@ -16,7 +21,11 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState -from .full_hand import get_hand_calibration_profile, validate_compact_payload +from .full_hand import ( + get_hand_calibration_profile, + infer_compact_payload_layout, + validate_compact_payload, +) G20_COMMAND_NAMES: tuple[str, ...] = ( @@ -71,7 +80,7 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = ( class CalibratedCommandMapper: - """Validated, side-specific lookup from a G20 command to URDF radians.""" + """Validated, side-specific lookup from G20 u8 values to URDF radians.""" def __init__( self, payload: Mapping[str, Any], *, expected_side: str | None = None @@ -86,9 +95,20 @@ class CalibratedCommandMapper: quality = payload["quality"] if quality.get("passed") is not True: raise ValueError("calibration quality.passed must be true") - profile = get_hand_calibration_profile(side) + layout_id = infer_compact_payload_layout(payload) + profile = get_hand_calibration_profile(side, layout_id) self.side = side + self.layout_id = layout_id self.serial_number = str(payload["serial_number"]) + schema_version = int(payload["schema_version"]) + self.input_domain = str( + payload.get( + "curve_input_domain", + "command_u8" if schema_version == 4 else "", + ) + ) + if self.input_domain not in {"command_u8", "feedback_u8"}: + raise ValueError("calibration curve_input_domain is invalid") self._motor_by_joint = { name: int(profile.joint_specs[name].motor_index) for name in G20_URDF_JOINT_NAMES @@ -100,6 +120,27 @@ class CalibratedCommandMapper: ) for name in G20_URDF_JOINT_NAMES } + self._decreasing_curves = { + name: tuple( + float(value) + for value in payload["joints"][name].get( + "decreasing_rad", payload["joints"][name]["angle_rad"] + ) + ) + for name in G20_URDF_JOINT_NAMES + } + self._increasing_curves = { + name: tuple( + float(value) + for value in payload["joints"][name].get( + "increasing_rad", payload["joints"][name]["angle_rad"] + ) + ) + for name in G20_URDF_JOINT_NAMES + } + self._previous_by_motor: dict[int, float] = {} + self._direction_by_motor: dict[int, str] = {} + self.direction_deadband_u8 = 0.5 @staticmethod def _command_index(value: float) -> int: @@ -133,10 +174,34 @@ class CalibratedCommandMapper: ) command = values indices = tuple(self._command_index(value) for value in command) - return tuple( - self._curves[name][indices[self._motor_by_joint[name]]] - for name in G20_URDF_JOINT_NAMES - ) + direction_by_motor: dict[int, str | None] = {} + for motor, value in enumerate(command): + previous = self._previous_by_motor.get(motor) + direction = self._direction_by_motor.get(motor) + if previous is not None: + if value > previous + self.direction_deadband_u8: + direction = "increasing" + elif value < previous - self.direction_deadband_u8: + direction = "decreasing" + direction_by_motor[motor] = direction + result: list[float] = [] + for name in G20_URDF_JOINT_NAMES: + motor = self._motor_by_joint[name] + direction = direction_by_motor[motor] + curves = ( + self._increasing_curves + if direction == "increasing" + else self._decreasing_curves + if direction == "decreasing" + else self._curves + ) + result.append(curves[name][indices[motor]]) + for motor, value in enumerate(command): + self._previous_by_motor[motor] = value + direction = direction_by_motor[motor] + if direction is not None: + self._direction_by_motor[motor] = direction + return tuple(result) def load_calibrated_command_mapper( @@ -149,6 +214,17 @@ def load_calibrated_command_mapper( return CalibratedCommandMapper(payload, expected_side=expected_side) +def default_input_topic(hand_type: str, input_domain: str) -> str: + side = str(hand_type).lower() + if side not in {"left", "right"}: + raise ValueError("hand_type must be left or right") + if input_domain == "feedback_u8": + return f"/g20/cb_{side}_hand_state" + if input_domain == "command_u8": + return f"/g20/cb_{side}_hand_control_cmd" + raise ValueError("calibration curve_input_domain is invalid") + + class CalibratedJointStateBridge(Node): def __init__(self) -> None: super().__init__("g20_calibrated_joint_state_bridge") @@ -168,7 +244,9 @@ class CalibratedJointStateBridge(Node): ) input_topic = str(self.get_parameter("input_topic").value).strip() output_topic = str(self.get_parameter("output_topic").value).strip() - self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd" + self.input_topic = input_topic or default_input_topic( + hand_type, self.mapper.input_domain + ) self.output_topic = ( output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state" ) @@ -179,7 +257,8 @@ class CalibratedJointStateBridge(Node): self._last_error = "" self.get_logger().info( f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: " - f"{self.input_topic} -> {self.output_topic}" + f"{self.input_topic} ({self.mapper.input_domain}) -> " + f"{self.output_topic}" ) def _command_callback(self, command: JointState) -> None: 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 63c9e70..54bdab4 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 @@ -10,7 +10,10 @@ from __future__ import annotations from dataclasses import dataclass, replace import math -from typing import Any, Mapping, Sequence +from pathlib import Path +import re +from typing import Any, Iterable, Mapping, Sequence +import xml.etree.ElementTree as ET import numpy as np @@ -58,6 +61,15 @@ class SweepSpec: view: str motor_index: int joints: tuple[str, ...] + task_name: str = "" + auxiliary_commands: tuple[tuple[int, int], ...] = () + validation_only: bool = False + + @property + def key(self) -> str: + if self.task_name: + return self.task_name + return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" @dataclass(frozen=True) @@ -83,6 +95,9 @@ class HandCalibrationProfile: image_trajectory_joints: frozenset[str] roll_clearance_commands: Mapping[int, int] thumb_pitch_clearance_commands: Mapping[int, int] + layout_id: str = "legacy_11" + measurement_specs: Mapping[str, JointSpec] | None = None + axis_validation_sources: Mapping[str, str] | None = None @property def measured_joints(self) -> tuple[str, ...]: @@ -90,6 +105,16 @@ class HandCalibrationProfile: name for name, spec in self.joint_specs.items() if spec.measured ) + @property + def record_specs(self) -> Mapping[str, JointSpec]: + return self.joint_specs if self.measurement_specs is None else self.measurement_specs + + @property + def record_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.record_specs.items() if spec.measured + ) + @property def active_joints(self) -> tuple[str, ...]: return tuple( @@ -122,6 +147,19 @@ class HandCalibrationProfile: _FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky") + +# The side camera looks almost along these physical flexion axes. The child +# Tag centre therefore traces a high-resolution image-plane circle against the +# fixed palm Tag, while monocular planar-PnP tilt is not accurate enough to be +# the dynamic angle source. Passive DIP is intentionally excluded because +# its parent Tag moves with PIP; a raw image-centre difference would mix both +# joint angles. +RIGHT_19_END_ON_IMAGE_CURVE_JOINTS = frozenset( + { + *(f"{finger}_mcp_pitch" for finger in _FINGERS), + *(f"{finger}_pip" for finger in _FINGERS), + } +) _MOTOR_BY_JOINT: dict[str, int] = { **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)}, **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)}, @@ -299,8 +337,276 @@ LEFT_HAND_PROFILE = _build_hand_profile("left") RIGHT_HAND_PROFILE = _build_hand_profile("right") -def get_hand_calibration_profile(side: str) -> HandCalibrationProfile: +G20_RIGHT_15_LAYOUT = "g20_right_15" +# Retain the established Python symbol while the product layout moves from 19 +# to 15 physical Tags. Persisted/session-facing layout IDs use the new value, +# so old 19-Tag checkpoints cannot be resumed accidentally. +G20_RIGHT_19_LAYOUT = G20_RIGHT_15_LAYOUT + +# The three CMC axes keep the calibration domain that was proven on this hand +# in a609d521: dense relative-rotation sweeps with an isolated-cycle holdout. +# Monocular Tag translation is useful as a diagnostic, but is not an +# independent quality measurement for these short, strongly coupled links. +G20_REFERENCE_THUMB_CMC_JOINTS: frozenset[str] = frozenset( + {"thumb_cmc_pitch", "thumb_cmc_roll", "thumb_cmc_yaw"} +) + +G20_COMBINATION_REQUIRED_TARGET_KEYS: tuple[str, ...] = tuple( + sorted( + { + "front:index_mcp_roll", + "front:middle_mcp_roll", + "front:ring_mcp_roll", + "front:pinky_mcp_roll", + "side:index_pip", + "side:middle_pip", + "side:ring_pip", + "side:pinky_pip", + } + ) +) + +MIMIC_DERIVED_FINGER_DIPS: Mapping[str, str] = { + f"{finger}_dip": f"{finger}_pip" for finger in _FINGERS +} + + +def canonical_zero_direction( + profile: HandCalibrationProfile, joint_name: str +) -> str | None: + """Return the repeatable approach direction used as physical zero. + + A mid-range encoder value does not uniquely determine a linkage pose when + the transmission has backlash. The right-hand product four-finger roll profile + therefore defines command 127 after a 255->127 approach as the sole + physical zero. The opposite branch remains offset in the published + direction-aware curve instead of being silently re-zeroed. + """ + name = str(joint_name) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return None + canonical = { + *(f"{finger}_mcp_roll" for finger in _FINGERS), + *(f"{finger}_mcp_roll_side" for finger in _FINGERS), + } + return "decreasing" if name in canonical else None + + +def _right_19_auxiliary_commands( + finger: str, *, side_view: bool, target_kind: str +) -> tuple[tuple[int, int], ...]: + """Return the reviewed right-hand fan/occlusion parking pose.""" + order = ("pinky", "ring", "middle", "index") + target_index = order.index(finger) + commands: dict[int, int] = {} + for other_index, other in enumerate(order): + if other == finger: + continue + roll_motor = _MOTOR_BY_JOINT[f"{other}_mcp_roll"] + if other_index < target_index: + commands[roll_motor] = 127 if side_view else 0 + else: + commands[roll_motor] = 255 + if side_view and other_index < target_index: + commands[_MOTOR_BY_JOINT[f"{other}_mcp_pitch"]] = 0 + commands[_MOTOR_BY_JOINT[f"{other}_pip"]] = 0 + if target_kind in {"pitch", "pip"}: + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]] = 127 + if target_kind == "roll": + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 + commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 + elif target_kind == "pitch": + commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 + elif target_kind == "pip": + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 + return tuple(sorted((int(index), int(value)) for index, value in commands.items())) + + +def _build_right_19_profile() -> HandCalibrationProfile: + """Build the 15-Tag/16-motion-task right-hand profile.""" + view_tags: dict[str, dict[str, int]] = { + "front": { + "front_base": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + "pinky_roll": 10, + "ring_roll": 11, + "middle_roll": 12, + "index_roll": 13, + }, + "side": { + "side_base": 4, + "ring_pip": 5, + "pinky_pip": 6, + "middle_pip": 15, + "index_pip": 17, + }, + "top": {"top_base": 8, "thumb_yaw": 9}, + } + # Four adjacent fingers inevitably occlude some Tags in the + # common open-hand pose. Startup therefore proves only the three fixed + # palm references. Every moving Tag is still mandatory, but is checked + # by its task-local visibility sweep after the other fingers have moved to + # that task's clearance pose. Requiring all 15 at one instant would make + # a physically correct installation impossible to start. + preflight_view_roles = { + "front": ("front_base",), + "side": ("side_base",), + "top": ("top_base",), + } + joint_specs: dict[str, JointSpec] = { + "thumb_cmc_pitch": JointSpec( + "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + ), + "thumb_cmc_roll": JointSpec( + "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + ), + "thumb_cmc_yaw": JointSpec( + "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", + zero_kind="urdf_axis_chain", + ), + "thumb_mcp": JointSpec( + "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", + zero_kind="urdf_axis_chain", + ), + "thumb_ip": JointSpec( + "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", + ), + } + for finger in ("index", "middle", "ring", "pinky"): + roll = f"{finger}_mcp_roll" + pitch = f"{finger}_mcp_pitch" + pip = f"{finger}_pip" + dip = f"{finger}_dip" + joint_specs[roll] = JointSpec( + roll, _MOTOR_BY_JOINT[roll], True, "front", "front_base", + f"{finger}_roll", zero_kind="urdf_axis_chain", + ) + # The side PIP Tag is on the middle phalanx after PIP. With PIP held + # at 255 it observes MCP pitch; with MCP pitch held at 255 it observes + # PIP itself. + joint_specs[pitch] = JointSpec( + pitch, _MOTOR_BY_JOINT[pitch], True, "side", "side_base", + f"{finger}_pip", zero_kind="urdf_axis_chain", + ) + joint_specs[pip] = JointSpec( + pip, _MOTOR_BY_JOINT[pip], True, "side", "side_base", + f"{finger}_pip", zero_kind="urdf_axis_chain", + ) + joint_specs[dip] = JointSpec( + dip, _MOTOR_BY_JOINT[pip], False, None, None, None, + source_joint=pip, + ) + + measurement_specs = dict(joint_specs) + axis_validation_sources: dict[str, str] = {} + for finger in ("pinky", "ring", "middle", "index"): + canonical = f"{finger}_mcp_roll" + alias = f"{canonical}_side" + measurement_specs[alias] = JointSpec( + alias, + _MOTOR_BY_JOINT[canonical], + True, + "side", + "side_base", + f"{finger}_pip", + zero_kind="axis_cross_view_validation", + ) + axis_validation_sources[canonical] = alias + + sweeps: list[SweepSpec] = [ + SweepSpec( + "front", 0, ("thumb_cmc_pitch",), "thumb_cmc_pitch_front", + ((5, 255), (10, 255)), + ), + SweepSpec("front", 5, ("thumb_cmc_roll",), "thumb_cmc_roll_front"), + SweepSpec( + "front", 15, ("thumb_mcp", "thumb_ip"), "thumb_mcp_ip_front" + ), + SweepSpec( + "top", 10, ("thumb_cmc_yaw",), "thumb_cmc_yaw_top", ((5, 145),) + ), + ] + for finger in ("pinky", "ring", "middle", "index"): + roll_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_roll"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + sweeps.extend( + ( + SweepSpec( + "front", + roll_motor, + ( + f"{finger}_mcp_roll", + f"{finger}_mcp_roll_side", + ), + f"{finger}_roll_multiview", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="roll" + ), + ), + SweepSpec( + "side", + pitch_motor, + (f"{finger}_mcp_pitch",), + f"{finger}_pitch_side", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="pitch" + ), + ), + SweepSpec( + "side", + pip_motor, + (f"{finger}_pip",), + f"{finger}_pip_side", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="pip" + ), + ), + ) + ) + image_joints = frozenset( + { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + *(f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky")), + } + ) + return HandCalibrationProfile( + side="right", + reference_finger="pinky", + view_tags=view_tags, + preflight_view_roles=preflight_view_roles, + joint_specs=joint_specs, + sweep_specs=tuple(sweeps), + image_trajectory_joints=image_joints, + roll_clearance_commands={}, + thumb_pitch_clearance_commands={10: 255, 5: 255}, + layout_id=G20_RIGHT_19_LAYOUT, + measurement_specs=measurement_specs, + axis_validation_sources=axis_validation_sources, + ) + + +RIGHT_19_HAND_PROFILE = _build_right_19_profile() + + +def get_hand_calibration_profile( + side: str, layout_id: str = "legacy_11" +) -> HandCalibrationProfile: value = str(side).lower() + layout = str(layout_id).lower() + if layout == G20_RIGHT_19_LAYOUT: + if value != "right": + raise ValueError("g20_right_15 layout requires hand side right") + return RIGHT_19_HAND_PROFILE + if layout != "legacy_11": + raise ValueError(f"unknown G20 tag layout: {layout_id}") if value == "left": return LEFT_HAND_PROFILE if value == "right": @@ -308,6 +614,138 @@ def get_hand_calibration_profile(side: str) -> HandCalibrationProfile: raise ValueError("hand side must be left or right") +def derive_mimic_passive_fits( + source_urdf: str | Path, + fits: Mapping[str, JointCurveFit], + *, + profile: HandCalibrationProfile, +) -> dict[str, JointCurveFit]: + """Add four non-visual DIP curves using the source URDF mimic contract. + + The runtime curves are relative to the baseline command, so a URDF mimic + offset cancels while the multiplier scales the measured PIP motion. The + URDF itself remains byte-for-byte protected by publication checks. + """ + result = dict(fits) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return result + path = Path(source_urdf).expanduser().resolve() + if not path.is_file(): + raise ValueError(f"source URDF does not exist: {path}") + joints = { + str(node.get("name")): node + for node in ET.parse(path).getroot().findall("joint") + } + for passive_name, source_name in MIMIC_DERIVED_FINGER_DIPS.items(): + spec = profile.joint_specs[passive_name] + if spec.source_joint != source_name: + raise ValueError(f"{passive_name} has the wrong curve source") + if source_name not in result: + raise ValueError(f"missing measured source curve {source_name}") + joint = joints.get(passive_name) + mimic = None if joint is None else joint.find("mimic") + if mimic is None or mimic.get("joint") != source_name: + raise ValueError( + f"source URDF {passive_name} must mimic {source_name}" + ) + multiplier = float(mimic.get("multiplier", "1")) + offset = float(mimic.get("offset", "0")) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"source URDF {passive_name} mimic multiplier is invalid") + if not math.isfinite(offset): + raise ValueError(f"source URDF {passive_name} mimic offset is invalid") + source = result[source_name] + + def scaled(values: Sequence[float]) -> tuple[float, ...]: + return tuple(multiplier * float(value) for value in values) + + result[passive_name] = JointCurveFit( + angle_rad=scaled(source.angle_rad), + decreasing_rad=scaled(source.decreasing_rad), + increasing_rad=scaled(source.increasing_rad), + circle={ + "curve_source": source_name, + "source_urdf_mimic_multiplier": multiplier, + "source_urdf_mimic_offset_rad": offset, + "visual_measurement": False, + }, + maximum_monotonic_correction_rad=( + abs(multiplier) * source.maximum_monotonic_correction_rad + ), + maximum_hysteresis_rad=( + abs(multiplier) * source.maximum_hysteresis_rad + ), + quality={ + "mimic_multiplier": multiplier, + "mimic_offset_rad": offset, + "visual_measurement": 0.0, + }, + zero_offset_rad=multiplier * source.zero_offset_rad, + ) + if set(result) != set(profile.joint_specs): + missing = sorted(set(profile.joint_specs) - set(result)) + extra = sorted(set(result) - set(profile.joint_specs)) + raise ValueError( + f"runtime fit set is incomplete: missing={missing} extra={extra}" + ) + return result + + +def clamp_runtime_fits_to_urdf_limits( + source_urdf: str | Path, + fits: Mapping[str, JointCurveFit], +) -> dict[str, JointCurveFit]: + """Saturate only published/runtime curves at protected CAD limits. + + Raw trajectory fits and all fit/zero diagnostics remain untouched. The + returned transfer functions are the commands a URDF/MuJoCo runtime can + actually represent safely. + """ + path = Path(source_urdf).expanduser().resolve() + joints = { + str(node.get("name")): node + for node in ET.parse(path).getroot().findall("joint") + } + result: dict[str, JointCurveFit] = {} + for name, fit in fits.items(): + joint = joints.get(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") + 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") + + clipped_counts: dict[str, int] = {} + + def clipped(values: Sequence[float], branch: str) -> tuple[float, ...]: + source = np.asarray(values, dtype=float) + bounded = np.clip(source, lower, upper) + clipped_counts[branch] = int(np.count_nonzero(bounded != source)) + return tuple(float(value) for value in bounded) + + circle = dict(fit.circle) + bounded_angle = clipped(fit.angle_rad, "angle_rad") + bounded_decreasing = clipped(fit.decreasing_rad, "decreasing_rad") + bounded_increasing = clipped(fit.increasing_rad, "increasing_rad") + if any(clipped_counts.values()): + circle["runtime_urdf_limit_clipped_bins"] = clipped_counts + circle["runtime_urdf_limits_rad"] = [lower, upper] + result[name] = replace( + fit, + angle_rad=bounded_angle, + decreasing_rad=bounded_decreasing, + increasing_rad=bounded_increasing, + circle=circle, + ) + return result + + # Backwards-compatible aliases keep the existing left-hand API stable. VIEW_TAGS = LEFT_HAND_PROFILE.view_tags JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs @@ -349,6 +787,8 @@ def calibration_auxiliary_commands( profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> dict[int, int]: """Return motors that must remain fixed throughout one calibration task.""" + if spec.auxiliary_commands: + return {int(index): int(value) for index, value in spec.auxiliary_commands} if spec.motor_index == 0: return dict(profile.thumb_pitch_clearance_commands) if spec.motor_index == profile.reference_roll_motor: @@ -399,6 +839,279 @@ def build_calibration_motion_command( return result +def _append_single_motor_waypoint( + waypoints: list[tuple[int, ...]], current: list[int], motor: int, value: int +) -> None: + target = int(value) + if current[int(motor)] == target: + return + current[int(motor)] = target + waypoints.append(tuple(current)) + + +def build_calibration_preparation_waypoints( + spec: SweepSpec, + command_u8: int, + *, + current_command: Sequence[int], + baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, + parallel: bool = True, +) -> tuple[tuple[int, ...], ...]: + """Enter a task pose in the reviewed safe order. + + ``parallel`` merges the sequential entry into joint-class groups: all + neighbouring roll axes open together, then the clearance fingers flex + their PIPs together, then their MCP pitches, then any remaining auxiliary + motors, and the measured roll channel moves last. Every roll still opens + before any finger flexes, each finger's PIP still flexes before its + pitch, and the roll targets are unchanged, so no axis is forced across + its coupled mechanical boundary. + """ + if len(current_command) != 20: + raise ValueError("current_command must contain exactly 20 values") + final = build_calibration_motion_command( + spec, command_u8, baseline=baseline, profile=profile + ) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return (tuple(final),) + current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] + controlled_motors = { + int(joint.motor_index) for joint in profile.joint_specs.values() + } + # G20 feedback channels 11..14 are reserved and remain zero in the SDK. + # Preserve their reported values in transition vectors; waiting for the + # nominal 255 baseline on a non-existent actuator would deadlock before the + # first real motor is allowed to move. + for motor in range(20): + if motor not in controlled_motors: + final[motor] = current[motor] + auxiliary = calibration_auxiliary_commands(spec, profile=profile) + finger_order = ("pinky", "ring", "middle", "index") + handled: set[int] = set() + capture_views = { + profile.record_specs[joint_name].view for joint_name in spec.joints + } + target_roll_motor: int | None = None + lower_roll_motors: list[int] = [] + if "side" in capture_views: + target_finger = str(spec.joints[0]).split("_", 1)[0] + target_roll_motor = _MOTOR_BY_JOINT[f"{target_finger}_mcp_roll"] + lower_roll_motors = list(range(9, target_roll_motor, -1)) + if parallel: + roll_group: list[int] = [] + pip_group: list[int] = [] + pitch_group: list[int] = [] + if target_roll_motor is not None: + for motor in range(6, target_roll_motor): + if auxiliary.get(motor) == 255: + roll_group.append(motor) + handled.add(motor) + for roll_motor in lower_roll_motors: + roll_group.append(roll_motor) + handled.add(roll_motor) + finger = finger_order[9 - roll_motor] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + if auxiliary.get(pip_motor) == 0: + pip_group.append(pip_motor) + handled.add(pip_motor) + if auxiliary.get(pitch_motor) == 0: + pitch_group.append(pitch_motor) + handled.add(pitch_motor) + other_group = [ + motor + for motor in sorted(controlled_motors | set(auxiliary)) + if motor not in handled + and motor != spec.motor_index + and current[motor] != final[motor] + ] + waypoints: list[tuple[int, ...]] = [] + _append_class_waypoint(waypoints, current, final, roll_group) + _append_class_waypoint(waypoints, current, final, pip_group) + _append_class_waypoint(waypoints, current, final, pitch_group) + _append_class_waypoint(waypoints, current, final, other_group) + # The measured channel moves last, after the swept volume is clear. + _append_single_motor_waypoint( + waypoints, current, spec.motor_index, final[spec.motor_index] + ) + return tuple(waypoints or [tuple(final)]) + waypoints = [] + if "side" in capture_views: + # Side-view clearance never forces a neighbouring roll axis across its + # coupled mechanical boundary. Real sessions showed neighbouring + # roll axes saturating at feedback 64 (motor 9) and 58 (motor 8) when + # commanded to 0 in a coupled avoidance pose. Keep those roll axes at + # neutral 127 and remove occlusion only by flexing the neighbour. + for motor in range(6, target_roll_motor): + if auxiliary.get(motor) == 255: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + handled.add(motor) + for roll_motor in lower_roll_motors: + if roll_motor in auxiliary: + _append_single_motor_waypoint( + waypoints, current, roll_motor, final[roll_motor] + ) + handled.add(roll_motor) + finger = finger_order[9 - roll_motor] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + # Once the finger has rolled away from its neighbour, flex it out + # of the camera line of sight using the reviewed PIP->pitch order. + if auxiliary.get(pip_motor) == 0: + _append_single_motor_waypoint( + waypoints, current, pip_motor, final[pip_motor] + ) + handled.add(pip_motor) + if auxiliary.get(pitch_motor) == 0: + _append_single_motor_waypoint( + waypoints, current, pitch_motor, final[pitch_motor] + ) + handled.add(pitch_motor) + # Move the measured roll only after its neighbours have flexed out of + # its swept volume. Pitch/PIP tasks leave this roll neutral throughout. + if target_roll_motor == spec.motor_index: + _append_single_motor_waypoint( + waypoints, + current, + target_roll_motor, + final[target_roll_motor], + ) + handled.add(target_roll_motor) + for motor in sorted(auxiliary): + if motor not in handled and motor != spec.motor_index: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + if spec.motor_index not in handled: + _append_single_motor_waypoint( + waypoints, current, spec.motor_index, final[spec.motor_index] + ) + if not waypoints or waypoints[-1] != tuple(final): + # Normally every difference has already been visited. Keep this + # assertion-style fallback explicit so a future profile field cannot + # silently bypass the one-motor transition policy. + differing = [ + index for index, (left, right) in enumerate(zip(current, final)) + if index in controlled_motors and left != right + ] + for motor in differing: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + return tuple(waypoints or [tuple(final)]) + + +def _append_class_waypoint( + waypoints: list[tuple[int, ...]], + current: list[int], + target: Sequence[int], + motors: Iterable[int], +) -> None: + """Move one joint-class group of motors in a single parallel waypoint.""" + moved = False + for motor in motors: + if current[int(motor)] != target[int(motor)]: + current[int(motor)] = target[int(motor)] + moved = True + if moved: + waypoints.append(tuple(current)) + + +def build_calibration_return_waypoints( + target_command: Sequence[int], + *, + current_command: Sequence[int], + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, + anchor_roll_motor: int | None = None, + parallel: bool = True, +) -> tuple[tuple[int, ...], ...]: + """Leave a product-layout avoidance pose using the inverse safe sequence. + + ``parallel`` merges the reviewed sequential order into joint-class groups: + all differing rolls return to neutral together, then all differing MCP + pitches, then all differing PIPs, then the remaining controlled motors. + Every finger still unfolds only after every roll is back at neutral, each + finger's pitch still moves before its PIP, and simultaneous same-direction + roll motion removes the "one finger sweeps across parked neighbours" + stall scenario that motivated the anchored one-motor sequence. + """ + if len(current_command) != 20 or len(target_command) != 20: + raise ValueError("return commands must contain exactly 20 values") + target = [int(value) for value in target_command] + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return (tuple(target),) + current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] + controlled_motors = { + int(joint.motor_index) for joint in profile.joint_specs.values() + } + for motor in range(20): + if motor not in controlled_motors: + target[motor] = current[motor] + if parallel: + waypoints: list[tuple[int, ...]] = [] + _append_class_waypoint(waypoints, current, target, range(6, 10)) + _append_class_waypoint(waypoints, current, target, range(1, 5)) + _append_class_waypoint(waypoints, current, target, range(16, 20)) + _append_class_waypoint( + waypoints, + current, + target, + sorted( + motor + for motor in controlled_motors + if motor not in set(range(6, 10)) + | set(range(1, 5)) + | set(range(16, 20)) + ), + ) + return tuple(waypoints or [tuple(target)]) + waypoints = [] + handled: set[int] = set() + # All four roll channels return to neutral before any bent clearance + # finger is unfolded. In the reviewed product sequence the current/next + # target finger must move first, then neighbouring fingers move outward. + # Returning index first from an all-255 pinky task makes it converge into + # three still-parked fingers and can mechanically stall around command175. + roll_order: Sequence[int] = tuple(range(6, 10)) + anchor = None if anchor_roll_motor is None else int(anchor_roll_motor) + if profile.layout_id == G20_RIGHT_19_LAYOUT: + if anchor in range(6, 10): + roll_order = tuple( + sorted( + range(6, 10), + key=lambda motor: (abs(motor - anchor), -motor), + ) + ) + else: + roll_order = tuple(reversed(range(6, 10))) + for motor in roll_order: + if current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + handled.add(motor) + # Reverse the entry order, one finger at a time: roll, pitch, then PIP. + for finger in ("index", "middle", "ring", "pinky"): + for motor in ( + _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"], + _MOTOR_BY_JOINT[f"{finger}_pip"], + ): + if current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + handled.add(motor) + for motor in sorted(controlled_motors): + if motor not in handled and current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + return tuple(waypoints or [tuple(target)]) + + def build_calibration_speed_profile( spec: SweepSpec, *, @@ -416,13 +1129,12 @@ def build_calibration_speed_profile( ): raise ValueError("calibration speeds must be in [0, 255]") speeds = [normal] * 5 - if spec.motor_index == profile.reference_roll_motor: - speeds[profile.reference_speed_slot] = index_roll - elif spec.motor_index in { - profile.reference_pitch_motor, - profile.reference_pip_motor, - }: - speeds[profile.reference_speed_slot] = index_flex + if spec.motor_index in range(6, 10): + speeds[spec.motor_index - 5] = index_roll + elif spec.motor_index in range(1, 5): + speeds[spec.motor_index] = index_flex + elif spec.motor_index in range(16, 20): + speeds[spec.motor_index - 15] = index_flex return speeds @@ -673,6 +1385,56 @@ def measure_joint_observation( return measure_joint_vector(fit, vector_xyz_m) +def compare_cross_view_roll_curves( + primary: JointCurveFit, + secondary: JointCurveFit, + *, + maximum_rms_difference_rad: float = math.radians(1.0), + maximum_branch_gap_difference_rad: float = math.radians(0.3), +) -> dict[str, float]: + """Validate two independently fitted roll curves before axis fusion.""" + 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] = {} + 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") + rms = float(np.sqrt(np.mean(np.square(left - right)))) + metrics[f"{field_name}_rms_difference_rad"] = rms + if rms > maximum_rms_difference_rad: + raise ValueError( + "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] + ) + secondary_gap = float( + secondary.increasing_rad[secondary_zero] + - secondary.decreasing_rad[secondary_zero] + ) + branch_gap_difference = abs(primary_gap - secondary_gap) + metrics["baseline_branch_gap_difference_rad"] = branch_gap_difference + if 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" + ) + return metrics + + def fit_projected_zero( records: Sequence[Mapping[str, Any]], *, @@ -742,10 +1504,23 @@ def build_compact_payload( passed: bool, baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, side: str = "left", + layout_id: str = "legacy_11", + zero_uncertainty_rad: Mapping[str, float] | None = None, + zero_cycle_offsets_rad: Mapping[str, Sequence[float]] | None = None, + zero_observers: Mapping[str, str] | None = None, + artifact_hashes: Mapping[str, str] | None = None, + cross_view_roll_metrics: Mapping[str, Mapping[str, float]] | None = None, + joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]] | None = None, + zero_geometry_diagnostics: Mapping[str, Any] | None = None, ) -> dict[str, Any]: - profile = get_hand_calibration_profile(side) - if set(measured_fits) != set(profile.measured_joints): - raise ValueError("measured_fits must contain all directly measured joints") + profile = get_hand_calibration_profile(side, layout_id) + expected_fits = ( + set(profile.joint_specs) + if profile.layout_id == G20_RIGHT_19_LAYOUT + else set(profile.measured_joints) + ) + if set(measured_fits) != expected_fits: + raise ValueError("measured_fits has the wrong runtime joint set") expected_active = { name for name, spec in profile.joint_specs.items() if spec.active } @@ -754,6 +1529,24 @@ def build_compact_payload( if len(baseline) != 20: raise ValueError("baseline must contain exactly 20 commands") + if profile.layout_id == G20_RIGHT_19_LAYOUT: + return _build_right_19_compact_payload( + serial_number=serial_number, + measured_fits=measured_fits, + urdf_zero_offsets_rad=urdf_zero_offsets_rad, + validation_errors_rad=validation_errors_rad, + passed=passed, + baseline=baseline, + profile=profile, + zero_uncertainty_rad=zero_uncertainty_rad or {}, + zero_cycle_offsets_rad=zero_cycle_offsets_rad or {}, + zero_observers=zero_observers or {}, + artifact_hashes=artifact_hashes or {}, + cross_view_roll_metrics=cross_view_roll_metrics or {}, + joint_dynamic_diagnostics=joint_dynamic_diagnostics or {}, + zero_geometry_diagnostics=zero_geometry_diagnostics or {}, + ) + joints: dict[str, dict[str, Any]] = {} for name, spec in profile.joint_specs.items(): source_name = spec.source_joint or name @@ -801,7 +1594,113 @@ def build_compact_payload( return payload +def _build_right_19_compact_payload( + *, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + urdf_zero_offsets_rad: Mapping[str, float], + validation_errors_rad: Sequence[float], + passed: bool, + baseline: Sequence[int], + profile: HandCalibrationProfile, + zero_uncertainty_rad: Mapping[str, float], + zero_cycle_offsets_rad: Mapping[str, Sequence[float]], + zero_observers: Mapping[str, str], + artifact_hashes: Mapping[str, str], + cross_view_roll_metrics: Mapping[str, Mapping[str, float]], + joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]], + zero_geometry_diagnostics: Mapping[str, Any], +) -> dict[str, Any]: + # The public runtime artifact intentionally keeps the established schema-v4 + # contract. Directional curves, uncertainty, hashes and geometry metrics + # are calibration diagnostics and belong in calibration_summary_zh.json; + # exposing them here made it too easy for a runtime to apply a feedback + # curve or a zero offset twice. + del ( + zero_uncertainty_rad, + zero_cycle_offsets_rad, + zero_observers, + artifact_hashes, + cross_view_roll_metrics, + joint_dynamic_diagnostics, + zero_geometry_diagnostics, + ) + joints: dict[str, dict[str, Any]] = {} + for name, spec in profile.joint_specs.items(): + fit = measured_fits[name] + zero_command = int(baseline[spec.motor_index]) + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + if decreasing.shape != (256,) or increasing.shape != (256,): + raise ValueError(f"{name} directional curves must contain 256 values") + # A single schema-v4 curve is the steady-state midpoint of both + # approach directions. Publication separately rejects a branch gap + # above 2 degrees, bounding the midpoint's direction error to 1 degree. + curve = 0.5 * (decreasing + increasing) + curve -= float(curve[zero_command]) + joint: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in curve], + } + if spec.active: + joint["zero_command_u8"] = zero_command + joint["zero_angles"] = { + "urdf_zero_offset_rad": round( + float(urdf_zero_offsets_rad[name]), 8 + ) + } + else: + joint["passive"] = True + joints[name] = joint + + errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) + mae = float(np.mean(errors)) if errors.size else float("nan") + p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") + payload = { + "schema_version": 4, + "model": "G20", + "side": "right", + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline], + "joints": joints, + "quality": { + "passed": bool(passed), + "validation_mae_rad": ( + None if not math.isfinite(mae) else round(mae, 8) + ), + "validation_p95_rad": ( + None if not math.isfinite(p95) else round(p95, 8) + ), + }, + } + validate_compact_payload(payload) + return payload + + +def infer_compact_payload_layout(payload: Mapping[str, Any]) -> str: + """Infer the layout for schema-v4 while honoring explicit newer schemas.""" + explicit = str(payload.get("layout_id", "")).strip().lower() + if explicit: + return explicit + raw_joints = payload.get("joints", {}) + independent_right = bool( + str(payload.get("side", "")).lower() == "right" + and isinstance(raw_joints, Mapping) + and raw_joints + and all( + not isinstance(value, Mapping) or "source_joint" not in value + for value in raw_joints.values() + ) + ) + return G20_RIGHT_19_LAYOUT if independent_right else "legacy_11" + + def validate_compact_payload(payload: Mapping[str, Any]) -> None: + if payload.get("schema_version") == 5: + _validate_right_19_compact_payload(payload) + return expected_top = { "schema_version", "model", "side", "serial_number", "angle_unit", "command_range", "baseline_command_u8", "joints", "quality", @@ -812,7 +1711,15 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: raise ValueError("schema_version must be 4") if payload["model"] != "G20" or payload["side"] not in {"left", "right"}: raise ValueError("payload must describe a left or right G20") - profile = get_hand_calibration_profile(str(payload["side"])) + # schema-v4 has no layout field. A right-hand payload without inherited + # source_joint fields is the 15-Tag product layout; legacy + # payloads retain their source_joint markers and remain readable. + layout_id = infer_compact_payload_layout(payload) + independent_right = layout_id == G20_RIGHT_19_LAYOUT + profile = get_hand_calibration_profile( + str(payload["side"]), + layout_id, + ) if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: raise ValueError("payload angle or command units are invalid") baseline = payload["baseline_command_u8"] @@ -828,7 +1735,7 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: allowed.add("zero_command_u8" if spec.active else "passive") if spec.active: allowed.add("zero_angles") - if spec.source_joint is not None: + if spec.source_joint is not None and not independent_right: allowed.add("source_joint") if set(joint) != allowed: raise ValueError(f"{name} has unexpected fields") @@ -859,7 +1766,7 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: ) elif joint.get("passive") is not True: raise ValueError(f"{name} must be marked passive") - if spec.source_joint is not None: + if spec.source_joint is not None and not independent_right: if joint.get("source_joint") != spec.source_joint: raise ValueError(f"{name} has the wrong source_joint") source = joints[spec.source_joint] @@ -876,3 +1783,203 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: "passed", "validation_mae_rad", "validation_p95_rad" }: raise ValueError("quality must contain only compact summary fields") + + +def _validate_right_19_compact_payload(payload: Mapping[str, Any]) -> None: + expected_top = { + "schema_version", "layout_id", "model", "side", "serial_number", + "angle_unit", "command_range", "curve_input_domain", + "runtime_curve_policy", "baseline_command_u8", + "static_zero_targets", "frozen_static_zero_joints", "joints", + "cross_view_roll", "static_zero_validation", "artifacts", "quality", + } + if set(payload) != expected_top: + raise ValueError("schema v5 calibration has unexpected top-level fields") + if ( + payload["layout_id"] not in {"g20_right_19", G20_RIGHT_15_LAYOUT} + or payload["model"] != "G20" + or payload["side"] != "right" + ): + raise ValueError("schema v5 must describe a G20 right product layout") + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = payload["baseline_command_u8"] + if not isinstance(baseline, list) or len(baseline) != 20: + raise ValueError("baseline_command_u8 must contain 20 values") + if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: + raise ValueError("payload angle or command units are invalid") + if payload["curve_input_domain"] != "feedback_u8": + raise ValueError("schema v5 curves must be feedback-indexed") + runtime_curve_policy = payload["runtime_curve_policy"] + if runtime_curve_policy not in { + "direction_aware", + "direction_aware_canonical_zero", + }: + raise ValueError("schema v5 runtime curve policy must be direction-aware") + # v5 is a read-only legacy contract from the earlier 15-zero product. + # Keep its historical scope stable even though new v4 publications now + # calibrate thumb_mcp as the 16th active static zero. + static_targets = { + name + for name, spec in profile.joint_specs.items() + if spec.zero_kind == "urdf_axis_chain" + } - {"thumb_mcp"} + frozen = {"thumb_mcp", *profile.passive_joints} + if set(payload["static_zero_targets"]) != static_targets: + raise ValueError("schema v5 must contain exactly 15 static-zero targets") + if set(payload["frozen_static_zero_joints"]) != frozen: + raise ValueError("schema v5 frozen static-zero joint set is invalid") + static_validation = payload["static_zero_validation"] + training_cycles = static_validation.get("training_cycles", []) + holdout_cycle = static_validation.get("holdout_cycle") + expected_cycle_count = len(training_cycles) + 1 + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): + raise ValueError("schema v5 must contain exactly 21 independent joints") + for name, spec in profile.joint_specs.items(): + joint = joints[name] + expected_fields = { + "motor_index", "zero_command_u8", "angle_rad", "decreasing_rad", + "increasing_rad", "dynamic_quality", "static_zero_status", + "zero_angles" if spec.active else "passive", + } + if set(joint) != expected_fields: + raise ValueError(f"{name} has unexpected schema v5 fields") + if int(joint["motor_index"]) != spec.motor_index: + raise ValueError(f"{name} has the wrong motor_index") + zero_command = joint["zero_command_u8"] + if zero_command != int(baseline[spec.motor_index]): + raise ValueError(f"{name} has the wrong zero command") + for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): + curve = np.asarray(joint[field_name], dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name}.{field_name} must contain 256 values") + if np.any(np.diff(curve) > 1.0e-7): + raise ValueError(f"{name}.{field_name} must be non-increasing") + allow_directional_offset = ( + runtime_curve_policy == "direction_aware_canonical_zero" + and canonical_zero_direction(profile, name) is not None + and field_name == "increasing_rad" + ) + if ( + not allow_directional_offset + and abs(float(curve[zero_command])) > 1.0e-6 + ): + raise ValueError(f"{name}.{field_name} must be zero at baseline") + expected_status = ( + "static_zero_calibrated" + if name in static_targets + else "cad_static_zero_retained_front_only" + if name == "thumb_mcp" + else "dynamic_measured_static_unobservable" + ) + if joint["static_zero_status"] != expected_status: + raise ValueError(f"{name} has the wrong static-zero status") + dynamic_quality = joint["dynamic_quality"] + if set(dynamic_quality) != { + "maximum_monotonic_correction_rad", + "maximum_hysteresis_rad", + "fit", + "cycle_travel_rad", + "cycle_travel_range_rad", + "baseline_hysteresis_by_cycle_rad", + "holdout_cycle", + "holdout_cycle_mae_rad", + "holdout_cycle_max_rad", + }: + raise ValueError(f"{name}.dynamic_quality fields are invalid") + if ( + dynamic_quality["holdout_cycle"] != holdout_cycle + or len(dynamic_quality["cycle_travel_rad"]) != expected_cycle_count + or len( + dynamic_quality["baseline_hysteresis_by_cycle_rad"] + ) != expected_cycle_count + ): + raise ValueError(f"{name} must record all dynamic cycles") + if spec.active: + zero = joint["zero_angles"] + if set(zero) != { + "urdf_zero_offset_rad", + "confidence_interval_95_half_width_rad", + "cycle_offsets_rad", + "observer_joint", + }: + raise ValueError(f"{name}.zero_angles fields are invalid") + if not math.isfinite(float(zero["urdf_zero_offset_rad"])): + raise ValueError(f"{name} zero offset is invalid") + if name in static_targets: + confidence = zero["confidence_interval_95_half_width_rad"] + if ( + confidence is None + or not math.isfinite(float(confidence)) + or not 0.0 <= float(confidence) <= math.radians(0.5) + ): + raise ValueError(f"{name} confidence interval is invalid") + if len(zero["cycle_offsets_rad"]) != len(training_cycles): + raise ValueError( + f"{name} must record training-only zero estimates" + ) + if not isinstance(zero["observer_joint"], str): + raise ValueError(f"{name} must record its zero observer") + if name == "thumb_mcp" and float(zero["urdf_zero_offset_rad"]) != 0.0: + raise ValueError("thumb_mcp static zero must remain source CAD") + elif joint.get("passive") is not True: + raise ValueError(f"{name} must be marked passive") + artifacts = payload["artifacts"] + expected_hashes = { + "source_urdf_sha256", "camera_extrinsics_sha256", + "corrected_urdf_sha256", + } + if set(artifacts) != expected_hashes or any( + re.fullmatch(r"[0-9a-f]{64}", str(value)) is None + for value in artifacts.values() + ): + raise ValueError("schema v5 artifact SHA-256 values are invalid") + if set(payload["cross_view_roll"]) != { + f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky") + }: + raise ValueError("schema v5 requires four cross-view roll diagnostics") + if set(static_validation) != { + "training_cycles", "holdout_cycle", "axis_line_rms_m", + "axis_line_rms_by_joint_m", "observability_rank", + "observability_parameter_count", "observability_condition_number", + "offset_covariance_rad2", + }: + raise ValueError("schema v5 static-zero validation fields are invalid") + training_cycles = static_validation["training_cycles"] + holdout_cycle = static_validation["holdout_cycle"] + if ( + not isinstance(training_cycles, list) + or len(training_cycles) < 3 + or any(not isinstance(value, int) for value in training_cycles) + or not isinstance(holdout_cycle, int) + or holdout_cycle in training_cycles + ): + raise ValueError("schema v5 training/holdout cycle split is invalid") + axis_line_rms = float(static_validation["axis_line_rms_m"]) + if not math.isfinite(axis_line_rms) or axis_line_rms < 0.0: + raise ValueError("schema v5 axis-line RMS is invalid") + line_by_joint = static_validation["axis_line_rms_by_joint_m"] + if not isinstance(line_by_joint, Mapping) or any( + not math.isfinite(float(value)) or float(value) < 0.0 + for value in line_by_joint.values() + ): + raise ValueError("schema v5 per-joint axis-line RMS is invalid") + parameter_count = int(static_validation["observability_parameter_count"]) + rank = int(static_validation["observability_rank"]) + condition = float(static_validation["observability_condition_number"]) + if parameter_count != 21 or rank != parameter_count: + raise ValueError("schema v5 zero-observation Jacobian is rank deficient") + if not math.isfinite(condition) or condition < 1.0: + raise ValueError("schema v5 observability condition number is invalid") + offset_covariance = static_validation["offset_covariance_rad2"] + if set(offset_covariance) != static_targets or any( + not math.isfinite(float(value)) or float(value) < 0.0 + for value in offset_covariance.values() + ): + raise ValueError("schema v5 offset covariance is invalid") + quality = payload["quality"] + if set(quality) != { + "passed", "all_15_static_targets_required", "validation_mae_rad", + "validation_p95_rad", + } or quality["all_15_static_targets_required"] is not True: + raise ValueError("schema v5 quality summary is invalid") 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 e879a27..9425395 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 @@ -21,10 +21,18 @@ import yaml from .extrinsics import load_three_camera_extrinsics from .full_hand import ( + G20_REFERENCE_THUMB_CMC_JOINTS, + G20_RIGHT_19_LAYOUT, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, HandCalibrationProfile, JointCurveFit, build_calibration_motion_command, build_compact_payload, + canonical_zero_direction, + clamp_runtime_fits_to_urdf_limits, + compare_cross_view_roll_curves, + derive_mimic_passive_fits, + fit_joint_image_curve, get_hand_calibration_profile, validate_compact_payload, ) @@ -33,10 +41,12 @@ from .urdf_zero import ( JointAxisMeasurement, UrdfKinematicModel, _angles_from_state, + baseline_hysteresis_by_cycle_rad, + circle_direction_is_constrained, fit_joint_axis_measurement, fit_rotation_joint_curve, get_zero_calibration_profile, - rotation_curve_holdout_errors, + joint_curve_holdout_errors, solve_urdf_zero_offsets, write_zero_corrected_urdf, ) @@ -68,8 +78,35 @@ def _load_parameters(path: Path) -> dict[str, Any]: return dict(payload["g20_calibration"]["ros__parameters"]) +def _steady_records_in_feedback_domain( + records: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Mirror the live mechanical-backlash domain for offline replay.""" + result: list[dict[str, Any]] = [] + for source in records: + record = dict(source) + requested = int( + record.get("requested_command_u8", record.get("command_u8", -1)) + ) + record["command_u8"] = ( + requested + if requested in {0, 255} + else int( + np.clip( + np.rint(float(record.get("feedback_u8", requested))), + 0, + 255, + ) + ) + ) + result.append(record) + return result + + def _latest_attempt_records( rows: Sequence[Mapping[str, Any]], + *, + kind: str = "sample", ) -> dict[str, list[dict[str, Any]]]: """Reproduce the online retry buffer from append-only raw samples. @@ -78,7 +115,7 @@ def _latest_attempt_records( must therefore select the greatest attempt independently for each logical trajectory rather than mixing rejected attempts into the final fit. """ - samples = [dict(row) for row in rows if row.get("kind") == "sample"] + samples = [dict(row) for row in rows if row.get("kind") == kind] latest_attempt: dict[tuple[str, int, str], int] = {} for row in samples: key = ( @@ -103,7 +140,13 @@ def _latest_attempt_records( def _load_raw_session( session_dir: Path, -) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]: +) -> tuple[ + 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" if not raw_path.is_file(): raise ValueError(f"raw session does not exist: {raw_path}") @@ -112,10 +155,27 @@ def _load_raw_session( for line in raw_path.read_text(encoding="utf-8").splitlines() if line.strip() ] + # New sessions persist both domains explicitly. The fitting primitives + # retain their historical command_u8 input name internally, so normalise + # only after loading; raw_samples.jsonl itself never contains an ambiguous + # command_u8 field. + for row in rows: + if "command_u8" in row: + continue + if row.get("kind") in {"sample", "baseline_hold_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"]) starts = [row for row in rows if row.get("kind") == "session_start"] if len(starts) != 1: raise ValueError("raw session must contain exactly one session_start") - return starts[0], _latest_attempt_records(rows), raw_path + return ( + starts[0], + _latest_attempt_records(rows), + _latest_attempt_records(rows, kind="baseline_hold_sample"), + _latest_attempt_records(rows, kind="steady_command_sample"), + raw_path, + ) def _fit_curve( @@ -125,39 +185,74 @@ def _fit_curve( profile: HandCalibrationProfile, baseline: Sequence[int], ) -> JointCurveFit: - motor = profile.joint_specs[name].motor_index + 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 + ): + return fit_joint_image_curve(records) return fit_rotation_joint_curve( records, zero_command_u8=int(baseline[motor]), + canonical_zero_direction=canonical_zero_direction(profile, name), ) +def _physical_rotation_axis_for_curve( + name: str, + records: Sequence[Mapping[str, Any]], + fit: JointCurveFit, + *, + profile: HandCalibrationProfile, + baseline: Sequence[int], +) -> Sequence[float]: + """Keep baseline pose hysteresis independent of the dynamic curve space.""" + axis = fit.circle.get("axis_xyz") + if axis is not None: + return axis + motor = profile.record_specs[name].motor_index + diagnostic_fit = fit_rotation_joint_curve( + records, + zero_command_u8=int(baseline[motor]), + canonical_zero_direction=canonical_zero_direction(profile, name), + ) + return diagnostic_fit.circle["axis_xyz"] + + def _fit_axes( *, records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], profile: HandCalibrationProfile, baseline: Sequence[int], extrinsics_file: Path, + source_urdf: Path, repetitions: int, ) -> list[JointAxisMeasurement]: - zero_profile = get_zero_calibration_profile(profile.side) + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) extrinsics = load_three_camera_extrinsics(extrinsics_file) cache: dict[tuple[str, int], JointAxisMeasurement] = {} - reference = profile.reference_finger upstream_by_joint = { "thumb_mcp": "thumb_cmc_pitch", "thumb_ip": "thumb_mcp", - f"{reference}_pip": f"{reference}_mcp_pitch", - f"{reference}_dip": f"{reference}_pip", + **{ + f"{finger}_pip": f"{finger}_mcp_pitch" + for finger in ("index", "middle", "ring", "pinky") + }, + **{ + f"{finger}_dip": f"{finger}_pip" + for finger in ("index", "middle", "ring", "pinky") + }, } - def fit_one(name: str, cycle: int) -> JointAxisMeasurement: + def fit_raw(name: str, cycle: int) -> JointAxisMeasurement: key = (name, cycle) if key in cache: return cache[key] upstream = upstream_by_joint.get(name) - constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz - spec = profile.joint_specs[name] + constraint = None if upstream is None else fit_raw(upstream, cycle).axis_common_xyz + spec = profile.record_specs[name] view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray( [0.0, 0.0, 1.0], dtype=float ) @@ -167,23 +262,152 @@ def _fit_axes( cycle=cycle, zero_command_u8=int(baseline[spec.motor_index]), axis_common_constraint=constraint, - constrained_circle_joints=zero_profile.constrained_circle_joints, + constrained_circle_joints=( + zero_profile.constrained_circle_joints + | ({name} if name.endswith("_side") else set()) + ), view_normal_common_xyz=view_normal, + canonical_zero_direction=canonical_zero_direction(profile, name), + ) + sweep_spec = next( + sweep for sweep in profile.sweep_specs if name in sweep.joints ) condition = build_calibration_motion_command( - spec, + sweep_spec, int(baseline[spec.motor_index]), baseline=baseline, profile=profile, ) result = replace( result, - condition_command_u8=tuple(float(value) for value in condition), + condition_command_u8=( + None + if profile.layout_id == G20_RIGHT_19_LAYOUT + else tuple(float(value) for value in condition) + ), view_normal_common_xyz=tuple(float(value) for value in view_normal), ) cache[key] = result return result + def fit_one(name: str, cycle: int) -> JointAxisMeasurement: + primary = fit_raw(name, cycle) + validation_name = (profile.axis_validation_sources or {}).get(name) + if validation_name is None: + return primary + secondary = fit_raw(validation_name, cycle) + primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) + secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) + if float(primary_axis @ secondary_axis) < 0.0: + secondary_axis = -secondary_axis + difference = math.acos( + float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) + ) + primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) + secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) + line_distance = float( + np.linalg.norm( + np.cross(secondary_point - primary_point, primary_axis) + ) + ) + # 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: + raise ValueError(f"{name}: cross-view axis gross disagreement") + if difference > math.radians(0.75) or line_distance > 0.001: + selected_direction = primary_axis + selected_source = primary.axis_direction_source + observer_name = next( + ( + observer + for observer, parent in zero_profile.axis_parent_joint.items() + if parent == name + ), + 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={} + ) + observer_axis, _ = model.axis_line( + observer_name, zero_offsets={}, joint_angles={} + ) + 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: + measured_cone = math.acos( + abs( + float( + np.clip( + candidate @ measured_observer, -1.0, 1.0 + ) + ) + ) + ) + return abs(measured_cone - expected_cone) + + if ( + cone_residual(primary_axis) > math.radians(5.0) + and cone_residual(secondary_axis) <= math.radians(5.0) + ): + selected_direction = secondary_axis + selected_source = "cross_view_cone_selected_secondary" + # Mirror the live node: use the view whose direction satisfies + # the zero-invariant parent/child cone, and take the physical line + # position from the side PIP-link circle. + return replace( + primary, + axis_common_xyz=tuple( + float(value) for value in selected_direction + ), + point_common_xyz_m=tuple( + float(value) + for value in secondary.point_common_xyz_m + ), + pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + axis_direction_source=selected_source, + axis_point_source="side_circle_cross_view", + ) + primary_variance = max( + primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + secondary_variance = max( + secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + primary_weight = 1.0 / primary_variance + secondary_weight = 1.0 / secondary_variance + fused_axis = ( + primary_weight * primary_axis + secondary_weight * secondary_axis + ) + fused_axis /= np.linalg.norm(fused_axis) + delta = secondary_point - primary_point + perpendicular = delta - fused_axis * float(delta @ fused_axis) + fused_point = primary_point + ( + secondary_weight / (primary_weight + secondary_weight) + ) * perpendicular + return replace( + primary, + axis_common_xyz=tuple(float(value) for value in fused_axis), + point_common_xyz_m=tuple(float(value) for value in fused_point), + pose_axis_line_rms_m=max( + primary.pose_axis_line_rms_m, + secondary.pose_axis_line_rms_m, + line_distance, + ), + axis_direction_source="cross_view_weighted_fusion", + ) + return [ fit_one(name, cycle) for name in zero_profile.axis_joints @@ -216,6 +440,9 @@ def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> floa def _quality_failures( *, records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + baseline_records_by_joint: Mapping[ + str, Sequence[Mapping[str, Any]] + ] | None = None, profile: HandCalibrationProfile, baseline: Sequence[int], fits: Mapping[str, JointCurveFit], @@ -224,7 +451,9 @@ def _quality_failures( ) -> list[str]: failures: list[str] = [] repetitions = int(parameters["repetitions"]) - zero_profile = get_zero_calibration_profile(profile.side) + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) axis_by_key = {(item.joint, item.cycle): item for item in axes} expected_directions = {"decreasing", "increasing"} for name in profile.measured_joints: @@ -277,17 +506,34 @@ def _quality_failures( failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms") spec = profile.joint_specs[name] fit = fits[name] - orthogonal_limit = math.radians( - float( - parameters[ - "active_maximum_rotation_orthogonal_rms_deg" - if spec.active - else "passive_maximum_rotation_orthogonal_rms_deg" - ] + if fit.circle.get("space") == "image_2d": + if float(fit.quality["radial_rms_px"]) > float( + parameters["image_trajectory_maximum_radial_rms_px"] + ): + failures.append(f"{name}: image radial RMS") + if float(fit.quality["radial_p95_px"]) > float( + parameters["image_trajectory_maximum_radial_p95_px"] + ): + failures.append(f"{name}: image radial P95") + if float(fit.quality["radius_px"]) < float( + parameters["image_trajectory_minimum_radius_px"] + ): + failures.append(f"{name}: image radius") + else: + orthogonal_limit = math.radians( + float( + parameters[ + "active_maximum_rotation_orthogonal_rms_deg" + if spec.active + else "passive_maximum_rotation_orthogonal_rms_deg" + ] + ) ) - ) - if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit: - failures.append(f"{name}: rotation orthogonal RMS") + if ( + float(fit.quality["rotation_orthogonal_rms_rad"]) + > orthogonal_limit + ): + failures.append(f"{name}: rotation orthogonal RMS") if float(fit.quality["arc_rad"]) < math.radians( float(parameters["trajectory_minimum_arc_deg"]) ): @@ -312,8 +558,56 @@ def _quality_failures( ) if fit.maximum_monotonic_correction_rad > monotonic_limit: failures.append(f"{name}: monotonic correction") - if fit.maximum_hysteresis_rad > hysteresis_limit: + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + and fit.maximum_hysteresis_rad > hysteresis_limit + ): failures.append(f"{name}: hysteresis") + if profile.layout_id == G20_RIGHT_19_LAYOUT: + hysteresis_records = records + dedicated = (baseline_records_by_joint or {}).get(name, ()) + if int(baseline[spec.motor_index]) not in {0, 255} and dedicated: + hysteresis_records = list(dedicated) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + hysteresis_records, + zero_command_u8=int(baseline[spec.motor_index]), + axis_xyz=_physical_rotation_axis_for_curve( + name, + records, + fit, + profile=profile, + baseline=baseline, + ), + ) + if canonical_zero_direction(profile, name) is not None: + gap_limit = math.radians( + float( + parameters.get( + "directional_zero_maximum_branch_gap_deg", 1.5 + ) + ) + ) + gap_range_limit = math.radians( + float( + parameters.get( + "directional_zero_maximum_branch_gap_range_deg", + 0.3, + ) + ) + ) + if max(baseline_hysteresis) > gap_limit: + failures.append(f"{name}: baseline directional gap") + if ( + max(baseline_hysteresis) - min(baseline_hysteresis) + > gap_range_limit + ): + failures.append( + f"{name}: baseline directional gap repeatability" + ) + elif max(baseline_hysteresis) > math.radians( + float(parameters.get("baseline_maximum_hysteresis_deg", 0.5)) + ): + failures.append(f"{name}: baseline hysteresis") cycle_travels: list[float] = [] cycle_axes: list[Sequence[float]] = [] @@ -333,13 +627,18 @@ def _quality_failures( cycle_axis_sources.append(axis.axis_direction_source) if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]): failures.append(f"{name} cycle {cycle + 1}: radial RMS") - if axis.pose_axis_line_rms_m > float( - parameters["axis_maximum_pose_line_rms_m"] + if ( + profile.record_specs[name].zero_kind + != "axis_cross_view_validation" + and axis.pose_axis_line_rms_m + > float(parameters["axis_maximum_pose_line_rms_m"]) ): failures.append( f"{name} cycle {cycle + 1}: pose axis-line RMS" ) - if name not in zero_profile.constrained_circle_joints: + if not circle_direction_is_constrained( + name, zero_profile.constrained_circle_joints + ): plane_limit = float( parameters[ "axis_maximum_plane_rms_m" @@ -350,8 +649,7 @@ def _quality_failures( if axis.plane_rms_m > plane_limit: failures.append(f"{name} cycle {cycle + 1}: plane RMS") if ( - name not in zero_profile.constrained_circle_joints - and axis.rotation_circle_axis_difference_rad > math.radians( + axis.rotation_circle_axis_difference_rad > math.radians( float(parameters["axis_maximum_rotation_circle_difference_deg"]) ) ): @@ -509,15 +807,94 @@ def replay_session( else Path(config_file).expanduser().resolve() ) parameters = _load_parameters(config) - start, records_by_joint, raw_path = _load_raw_session(session) + ( + start, + records_by_joint, + baseline_records_by_joint, + command_records_by_joint, + raw_path, + ) = _load_raw_session(session) side = str(start["hand_type"]).lower() - profile = get_hand_calibration_profile(side) - zero_profile = get_zero_calibration_profile(side) + layout_id = str(start.get("tag_layout", "legacy_11")).lower() + profile = get_hand_calibration_profile(side, layout_id) + zero_profile = get_zero_calibration_profile(side, layout_id) baseline = tuple(int(value) for value in start["baseline_command_u8"]) if len(baseline) != 20: raise ValueError("session baseline must contain exactly 20 commands") - if set(records_by_joint) != set(profile.measured_joints): - raise ValueError("raw session does not contain exactly the measured joint set") + if layout_id == G20_RIGHT_19_LAYOUT and write_outputs: + replay_rows = [ + json.loads(line) + for line in raw_path.read_text(encoding="utf-8").splitlines() + if line.strip() + ] + combinations = [ + row + for row in replay_rows + if row.get("kind") == "combination_validation_sample" + and bool(row.get("passed")) + ] + expected_poses = { + "all_open", "thumb_middle", "index_middle", "middle_middle", + "ring_middle", "pinky_middle", "half_grip", "light_pinch", + } + if {str(row.get("pose_name")) for row in combinations} != expected_poses: + raise ValueError( + "incomplete session cannot publish: eight combination poses are required" + ) + if any( + float(row.get("position_p95_m", float("inf"))) > 0.003 + or float(row.get("orientation_p95_rad", float("inf"))) + > math.radians(2.0) + 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") + + def hysteresis_records( + name: str, + ) -> Sequence[Mapping[str, Any]]: + spec = profile.record_specs[name] + zero = int(baseline[spec.motor_index]) + if ( + layout_id == G20_RIGHT_19_LAYOUT + and zero not in {0, 255} + and start.get("baseline_hysteresis_source") + == "dedicated_mid_sweep_hold" + ): + records = baseline_records_by_joint.get(name, []) + if not records: + raise ValueError( + f"{name} is missing dedicated baseline hold samples" + ) + return records + return records_by_joint[name] + + def model_records( + 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]] + if canonical_zero_direction(profile, name) is None: + return records + required = { + "relative_translation_xyz_m", + "parent_pose_common", + } + records.extend( + dict(record) + for record in baseline_records_by_joint.get(name, ()) + if not require_full_pose or required.issubset(record) + ) + return records + + curve_records_by_joint = { + name: model_records(name) for name in profile.record_joints + } + axis_records_by_joint = { + name: model_records(name, require_full_pose=True) + for name in profile.record_joints + } source_urdf = Path(start["source_urdf_path"]).expanduser().resolve() extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve() if not source_urdf.is_file() or not extrinsics_file.is_file(): @@ -526,39 +903,187 @@ def replay_session( raise ValueError("offline replay requires the original CAD URDF") hand_serial = str(serial_number or session.parent.name) output_suffix = _output_suffix(output_tag) - repetitions = int(parameters["repetitions"]) + repetitions = int( + parameters.get("g20_right_19_repetitions", 4) + if layout_id == G20_RIGHT_19_LAYOUT + else parameters["repetitions"] + ) measured_fits = { name: _fit_curve( name, - records_by_joint[name], + curve_records_by_joint[name], profile=profile, baseline=baseline, ) for name in profile.measured_joints } + validation_cycle = repetitions - 1 + training_cycles = tuple(range(validation_cycle)) + training_cycle_set = set(training_cycles) + if layout_id == G20_RIGHT_19_LAYOUT and len(training_cycles) < 3: + raise ValueError( + "g20_right_15 replay requires at least 3 training cycles and " + "one isolated holdout" + ) training_fits = { name: _fit_curve( name, [ record - for record in records_by_joint[name] - if int(record["cycle"]) in {0, 1} + for record in curve_records_by_joint[name] + if int(record["cycle"]) in training_cycle_set ], profile=profile, baseline=baseline, ) for name in profile.measured_joints } + command_fits = dict(training_fits) + if layout_id == G20_RIGHT_19_LAYOUT: + if set(command_records_by_joint) != set(profile.record_joints): + raise ValueError( + "g20_right_15 session is incomplete: missing steady command checkpoints" + ) + command_fits = {} + for name in profile.measured_joints: + if name in G20_REFERENCE_THUMB_CMC_JOINTS: + command_fits[name] = measured_fits[name] + continue + records = [ + dict(record) + for record in command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + if canonical_zero_direction(profile, name) is not None: + records.extend( + dict(record) + for record in baseline_records_by_joint.get(name, ()) + if int(record["cycle"]) == 0 + ) + if len(records) < 18: + raise ValueError( + f"{name} has {len(records)}/18 steady command checkpoints" + ) + command_fits[name] = _fit_curve( + name, records, profile=profile, baseline=baseline + ) + command_gap_limit = math.radians( + float(parameters.get("command_maximum_direction_gap_deg", 2.0)) + ) + command_feedback_fits = { + name: _fit_curve( + name, + _steady_records_in_feedback_domain( + [ + dict(record) + for record in command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + ), + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + if name not in G20_REFERENCE_THUMB_CMC_JOINTS + } + failed_command_gaps = { + name: fit.maximum_hysteresis_rad + for name, fit in command_feedback_fits.items() + if fit.maximum_hysteresis_rad > command_gap_limit + } + if failed_command_gaps: + raise ValueError( + "command-direction gap exceeds 2deg: " + + ", ".join( + f"{name}={math.degrees(value):.3f}deg" + for name, value in failed_command_gaps.items() + ) + ) + command_fits = clamp_runtime_fits_to_urdf_limits( + source_urdf, + derive_mimic_passive_fits( + source_urdf, + command_fits, + profile=profile, + ), + ) + cross_view_roll_metrics: dict[str, dict[str, float]] = {} + for name, validation_name in ( + profile.axis_validation_sources or {} + ).items(): + validation_training = _fit_curve( + validation_name, + [ + record + for record in curve_records_by_joint[validation_name] + if int(record["cycle"]) in training_cycle_set + ], + profile=profile, + baseline=baseline, + ) + validation_all = _fit_curve( + validation_name, + curve_records_by_joint[validation_name], + 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, + ) + ) + ), + ) + 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, + ) + ) + ), + ) + cross_view_roll_metrics[name] = { + **{ + f"training_{key}": value + for key, value in training_metrics.items() + }, + **{ + f"final_{key}": value + for key, value in final_metrics.items() + }, + } axes = _fit_axes( - records_by_joint=records_by_joint, + records_by_joint=axis_records_by_joint, profile=profile, baseline=baseline, extrinsics_file=extrinsics_file, + source_urdf=source_urdf, repetitions=repetitions, ) failures = _quality_failures( records_by_joint=records_by_joint, + baseline_records_by_joint=( + baseline_records_by_joint + if start.get("baseline_hysteresis_source") + == "dedicated_mid_sweep_hold" + else None + ), profile=profile, baseline=baseline, fits=measured_fits, @@ -569,12 +1094,12 @@ def replay_session( raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures)) holdout_by_joint = { - name: rotation_curve_holdout_errors( + name: joint_curve_holdout_errors( training_fits[name], [ record - for record in records_by_joint[name] - if int(record["cycle"]) == 2 + for record in curve_records_by_joint[name] + if int(record["cycle"]) == validation_cycle ], zero_command_u8=int( baseline[profile.joint_specs[name].motor_index] @@ -582,6 +1107,50 @@ def replay_session( ) for name in profile.measured_joints } + joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} + for name in profile.measured_joints: + cycle_travel: list[float] = [] + for cycle in range(repetitions): + cycle_fit = _fit_curve( + name, + [ + record + for record in records_by_joint[name] + if int(record["cycle"]) == cycle + ], + profile=profile, + baseline=baseline, + ) + cycle_travel.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + holdout = np.abs(np.asarray(holdout_by_joint[name], dtype=float)) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + hysteresis_records(name), + zero_command_u8=int( + baseline[profile.joint_specs[name].motor_index] + ), + axis_xyz=_physical_rotation_axis_for_curve( + name, + records_by_joint[name], + measured_fits[name], + profile=profile, + baseline=baseline, + ), + ) + joint_dynamic_diagnostics[name] = { + "cycle_travel_rad": [float(value) for value in cycle_travel], + "cycle_travel_range_rad": max(cycle_travel) - min(cycle_travel), + "baseline_hysteresis_by_cycle_rad": [ + float(value) for value in baseline_hysteresis + ], + "holdout_cycle": validation_cycle, + "holdout_cycle_mae_rad": float(np.mean(holdout)), + "holdout_cycle_max_rad": float(np.max(holdout)), + } trajectory_errors = np.abs( np.asarray( [value for values in holdout_by_joint.values() for value in values], @@ -599,8 +1168,29 @@ def replay_session( if ( trajectory_mae > maximum_validation_mae or trajectory_p95 > maximum_validation_p95 + or ( + layout_id == G20_RIGHT_19_LAYOUT + and float(np.max(trajectory_errors)) + > math.radians( + float(parameters.get("maximum_validation_error_deg", 1.5)) + ) + ) + or ( + layout_id == G20_RIGHT_19_LAYOUT + and any( + float(np.mean(np.abs(np.asarray(values, dtype=float)))) + > maximum_validation_mae + or float(np.max(np.abs(np.asarray(values, dtype=float)))) + > math.radians( + float( + parameters.get("maximum_validation_error_deg", 1.5) + ) + ) + for values in holdout_by_joint.values() + ) + ) ): - raise ValueError("third-cycle trajectory holdout failed") + raise ValueError("isolated trajectory holdout failed") motor_by_joint = { name: int(spec.motor_index) for name, spec in profile.joint_specs.items() @@ -621,12 +1211,38 @@ def replay_session( "maximum_axis_cone_mismatch_rad": math.radians( float(parameters["zero_maximum_axis_cone_mismatch_deg"]) ), + "maximum_observability_condition_number": float( + parameters.get( + "zero_maximum_observability_condition_number", 1.0e10 + ) + ), "maximum_pose_axis_line_rms_m": float( parameters["axis_maximum_pose_line_rms_m"] ), "maximum_validation_mae_rad": maximum_validation_mae, "maximum_validation_p95_rad": maximum_validation_p95, + "maximum_validation_error_rad": ( + math.radians( + float(parameters.get("maximum_validation_error_deg", 1.5)) + ) + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + "maximum_confidence_half_width_rad": ( + math.radians( + float( + parameters.get( + "zero_maximum_confidence_half_width_deg", 0.5 + ) + ) + ) + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), "hand_type": side, + "tag_layout": layout_id, + "training_cycles": training_cycles, + "validation_cycle": validation_cycle, } holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments) if not holdout_zero.passed: @@ -642,25 +1258,14 @@ def replay_session( }, } raise ValueError( - "third-cycle zero/URDF holdout failed: " + "isolated zero/URDF holdout failed: " + json.dumps(failure, ensure_ascii=False, sort_keys=True) ) - final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments) - if not final_zero.passed: - raise ValueError( - "all-cycle zero refit failed: " - + json.dumps( - { - "reasons": dict(final_zero.failure_reasons), - "fitted_offsets_deg": { - name: math.degrees(value) - for name, value in final_zero.direct_offsets_rad.items() - }, - }, - ensure_ascii=False, - sort_keys=True, - ) - ) + # Keep the model frozen after the holdout decision. Re-fitting with the + # held-out cycle would make the published artifact differ from the model + # that was independently evaluated above. + final_zero = holdout_zero + measured_fits = command_fits for target, source_name in zero_profile.inherited_static_zero_joints.items(): if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]: raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}") @@ -675,17 +1280,6 @@ def replay_session( float(value) for values in holdout_by_joint.values() for value in values ] validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad) - payload = build_compact_payload( - serial_number=hand_serial, - measured_fits=measured_fits, - urdf_zero_offsets_rad=final_zero.all_active_offsets_rad, - validation_errors_rad=validation_errors, - passed=True, - baseline=baseline, - side=side, - ) - validate_compact_payload(payload) - stamp = session.name final_json = session / ( f"g20_{side}_{hand_serial}_calibration{output_suffix}.json" @@ -694,7 +1288,7 @@ def replay_session( f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}" f"{output_suffix}.urdf" ) - final_urdf = source_urdf.parent / expected_urdf_name + final_urdf = session / expected_urdf_name report_path = session / ( f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json" ) @@ -707,25 +1301,42 @@ def replay_session( ) source_hash_before = _sha256(source_urdf) + published_zero_offsets = { + name: round(float(value), 8) + for name, value in final_zero.all_active_offsets_rad.items() + } + urdf_offsets = ( + { + name: published_zero_offsets[name] + for name in zero_profile.direct_zero_joints + } + if layout_id == G20_RIGHT_19_LAYOUT + else published_zero_offsets + ) with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary: candidate = write_zero_corrected_urdf( source_urdf=source_urdf, output_directory=temporary, serial_number=hand_serial, - offsets_rad=final_zero.all_active_offsets_rad, + offsets_rad=urdf_offsets, timestamp=stamp, ) urdf_checks = _validate_corrected_urdf( source=source_urdf, corrected=candidate, - offsets=final_zero.all_active_offsets_rad, + offsets=urdf_offsets, axes=axes, - curves=measured_fits, + curves=training_fits, motor_by_joint=motor_by_joint, inherited_zero_joints=zero_profile.inherited_zero_joints, ) residual_zero = solve_urdf_zero_offsets( - curves=measured_fits, + # Re-check the corrected URDF in the exact frozen trajectory + # domain that produced the accepted zero offsets. Runtime + # requested-command curves are a separate transfer function and + # differ slightly at settled checkpoints; using them here creates + # a fictitious residual zero after a structurally correct write. + curves=training_fits, fixed_direct_zero_offsets_rad={ name: 0.0 for name in zero_profile.fixed_direct_zero_offsets_rad @@ -740,10 +1351,88 @@ def replay_session( abs(float(value)) for value in residual_zero.direct_offsets_rad.values() ) if not residual_zero.passed or maximum_residual_offset > math.radians(0.3): - raise ValueError("written URDF retains a significant zero correction") + raise ValueError( + "written URDF retains a significant zero correction: " + + json.dumps( + { + "passed": residual_zero.passed, + "residual_offsets_deg": { + name: math.degrees(value) + for name, value in ( + residual_zero.direct_offsets_rad.items() + ) + }, + "failure_reasons": dict( + residual_zero.failure_reasons + ), + }, + ensure_ascii=False, + sort_keys=True, + ) + ) candidate_hash = _sha256(candidate) + payload = build_compact_payload( + serial_number=hand_serial, + measured_fits=command_fits, + urdf_zero_offsets_rad=published_zero_offsets, + validation_errors_rad=validation_errors, + passed=True, + baseline=baseline, + side=side, + layout_id=layout_id, + zero_uncertainty_rad=( + final_zero.offset_confidence_half_width_rad + ), + zero_cycle_offsets_rad=final_zero.cycle_offsets_rad, + zero_observers=zero_profile.offset_observer_joint, + artifact_hashes=( + { + "source_urdf_sha256": source_hash_before, + "camera_extrinsics_sha256": _sha256(extrinsics_file), + "corrected_urdf_sha256": candidate_hash, + } + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + cross_view_roll_metrics=cross_view_roll_metrics, + joint_dynamic_diagnostics=joint_dynamic_diagnostics, + zero_geometry_diagnostics=( + { + "training_cycles": final_zero.training_cycles, + "validation_cycle": final_zero.validation_cycle, + "axis_line_rms_m": final_zero.axis_line_rms_m, + "validation_line_error_by_joint_m": ( + final_zero.validation_line_error_by_joint_m + ), + "observability_rank": final_zero.observability_rank, + "observability_parameter_count": ( + final_zero.observability_parameter_count + ), + "observability_condition_number": ( + final_zero.observability_condition_number + ), + "offset_covariance_rad2": ( + final_zero.offset_covariance_rad2 + ), + } + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + ) + validate_compact_payload(payload) if write_outputs: + if _sha256(source_urdf) != source_hash_before: + raise ValueError("source URDF changed during offline replay") os.replace(candidate, final_urdf) + try: + # As in the live path, the atomically written JSON is the + # commit marker for the already validated URDF/JSON pair. + atomic_write_json(final_json, payload) + except Exception: + if layout_id == G20_RIGHT_19_LAYOUT and final_urdf.exists(): + rejected = final_urdf.with_suffix(".urdf.rejected") + final_urdf.replace(rejected) + raise if _sha256(source_urdf) != source_hash_before: raise ValueError("source URDF changed during offline replay") @@ -776,6 +1465,12 @@ def replay_session( name: math.degrees(value) for name, value in final_zero.direct_offsets_rad.items() }, + "model_base_translation_xyz_m": list( + final_zero.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + final_zero.base_quaternion_xyzw + ), "cycle_offsets_deg": { name: [math.degrees(value) for value in values] for name, values in holdout_zero.cycle_offsets_rad.items() @@ -809,7 +1504,6 @@ def replay_session( "corrected_urdf": str(final_urdf) if write_outputs else None, } if write_outputs: - atomic_write_json(final_json, payload) report["final_json_sha256"] = _sha256(final_json) if _sha256(final_urdf) != candidate_hash: raise ValueError("formal corrected URDF differs from validated candidate") 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 new file mode 100644 index 0000000..0a4f375 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py @@ -0,0 +1,553 @@ +"""One-command product runner for G20_RIGHT_001 calibration.""" + +from __future__ import annotations + +import argparse +from datetime import datetime +import json +import os +from pathlib import Path +import signal +import subprocess +import sys +import time +import traceback +from typing import Any, Mapping + +from ament_index_python.packages import get_package_share_directory +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from .hikrobot_camera import configure_fastdds_large_image_transport +from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh +from .product import ProductConfig, load_product_config, sha256_file +from .publication import atomic_session_pointer, finalize_session_artifacts + + +EXIT_PASS = 0 +EXIT_QUALITY = 2 +EXIT_SAFETY = 3 +STATUS_TIMEOUT_SECONDS = 90.0 + + +class CalibrationMonitor(Node): + def __init__(self) -> None: + super().__init__("g20_calibration_product_runner") + self.latest_status: dict[str, Any] = {} + self.last_status_at = time.monotonic() + self.start_requested = False + self.start_future: Any = None + self.abort_future: Any = None + self.create_subscription(String, "/g20_calibration/status", self._status, 10) + self.start_client = self.create_client(Trigger, "/g20_calibration/start") + self.abort_client = self.create_client(Trigger, "/g20_calibration/abort") + + def _status(self, message: String) -> None: + try: + payload = json.loads(message.data) + except (TypeError, json.JSONDecodeError): + return + if isinstance(payload, dict): + self.latest_status = payload + self.last_status_at = time.monotonic() + + def maybe_start(self) -> None: + if self.start_requested or self.latest_status.get("state") != "WAIT_START": + return + if not self.start_client.service_is_ready(): + self.start_client.wait_for_service(timeout_sec=0.05) + return + self.start_requested = True + self.start_future = self.start_client.call_async(Trigger.Request()) + + def abort(self) -> None: + if not self.abort_client.service_is_ready(): + self.abort_client.wait_for_service(timeout_sec=1.0) + if self.abort_client.service_is_ready(): + self.abort_future = self.abort_client.call_async(Trigger.Request()) + + +class ProgressConsole: + def __init__(self, serial_number: str) -> None: + self.serial_number = serial_number + self.estimator = ProgressEstimator.start() + self.last_text = "" + self.last_issue = "" + + def update(self, status: Mapping[str, Any]) -> None: + text = render_progress_zh(self.serial_number, status, self.estimator) + if text == self.last_text: + return + self.last_text = text + if sys.stdout.isatty(): + sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") + sys.stdout.flush() + else: + print(text, flush=True) + reason = str(status.get("reason", "")) + if reason.startswith("automatic_retry_") and reason != self.last_issue: + self.last_issue = reason + active = status.get("active", {}) + print( + "\n".join( + [ + f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}", + f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）", + ] + ), + flush=True, + ) + + +def _default_product_config() -> Path: + try: + installed = Path( + get_package_share_directory("g20_thumb_apriltag_calibration") + ) / "config" / "g20_right_product.yaml" + if installed.is_file(): + return installed + except Exception: + pass + return ( + Path.cwd() + / "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml" + ).resolve() + + +def _launch_command( + config: ProductConfig, + session: Path, + *, + resume_from: Path | None = None, +) -> list[str]: + values = { + "hand_type": "right", + "tag_layout": "g20_right_15", + "serial_number": config.serial_number, + "can_interface": config.can_interface, + "session_dir": str(session), + "output_root": str(config.output_root), + "camera_extrinsics_file": str(config.camera_extrinsics), + "source_urdf_path": str(config.source_urdf), + "source_urdf_expected_sha256": config.source_urdf_sha256, + "corrected_urdf_output_dir": str(session), + "calibration_config": str(config.calibration_config), + "tag_config": str(config.tag_config), + "commands_enabled": "true", + "start_cameras": "true", + "start_sdk": "true", + "record_bag": "false", + "validation_enabled": "false", + } + if resume_from is not None: + values["resume_raw_samples_path"] = str( + resume_from / "raw_samples.jsonl" + ) + for view, camera in config.cameras.items(): + values[f"{view}_camera_serial"] = camera["serial_number"] + values[f"{view}_camera_name"] = camera["camera_name"] + values[f"{view}_camera_info_url"] = camera["camera_info"] + return [ + "ros2", + "launch", + "g20_thumb_apriltag_calibration", + "three_camera_calibration.launch.py", + *(f"{name}:={value}" for name, value in values.items()), + ] + + +def _stop_stack(process: subprocess.Popen[Any]) -> None: + if process.poll() is not None: + return + try: + os.killpg(process.pid, signal.SIGINT) + except ProcessLookupError: + return + try: + process.wait(timeout=15.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGTERM) + except ProcessLookupError: + return + try: + process.wait(timeout=5.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGKILL) + except ProcessLookupError: + return + process.wait(timeout=5.0) + + +def _write_trace(log_path: Path, error: BaseException) -> None: + with log_path.open("a", encoding="utf-8") as stream: + stream.write("\n[one-command exception]\n") + traceback.print_exception(type(error), error, error.__traceback__, file=stream) + + +def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None: + monitor.abort() + deadline = time.monotonic() + float(timeout_seconds) + while time.monotonic() < deadline and rclpy.ok(): + rclpy.spin_once(monitor, timeout_sec=0.1) + if monitor.latest_status.get("state") == "ABORTED": + return + + +def _run_hardware_session( + config: ProductConfig, + session: Path, + *, + resume_from: Path | None = None, +) -> tuple[dict[str, Any], int]: + session.mkdir(parents=True, exist_ok=False) + (session / "raw_samples.jsonl").touch() + log_path = session / "calibration.log" + log_stream = log_path.open("a", encoding="utf-8", buffering=1) + atomic_session_pointer(config.session_root, "latest_attempt", session) + monitor = CalibrationMonitor() + console = ProgressConsole(config.serial_number) + process: subprocess.Popen[Any] | None = None + latest_status: dict[str, Any] = { + "state": "PREFLIGHT", + "reason": "starting_ros_stack", + "progress": 0.0, + "views": {}, + "feedback_hz": 0.0, + } + exit_code = EXIT_QUALITY + try: + process = subprocess.Popen( + _launch_command(config, session, resume_from=resume_from), + cwd=config.workspace, + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + launched_at = time.monotonic() + last_render = 0.0 + while True: + rclpy.spin_once(monitor, timeout_sec=0.1) + if monitor.latest_status: + latest_status = monitor.latest_status + monitor.maybe_start() + now = time.monotonic() + if now - last_render >= 0.5: + console.update(latest_status) + last_render = now + if monitor.start_future is not None and monitor.start_future.done(): + response = monitor.start_future.result() + if response is None or not response.success: + message = "start service failed" if response is None else response.message + raise RuntimeError(f"CFG-START-008:{message}") + monitor.start_future = None + state = str(latest_status.get("state", "")) + if state == "COMPLETE": + exit_code = EXIT_PASS + break + if state in {"PAUSED", "ABORTED"}: + reason = str(latest_status.get("reason", "calibration_paused")) + exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY + if state == "PAUSED" and "stall" not in reason: + # Ordinary quality failures return to the reviewed baseline + # before the process tree is stopped. Mechanical stalls + # deliberately skip this path and keep the current pose. + failure_status = dict(latest_status) + _request_safe_abort(monitor) + latest_status = failure_status + break + if process.poll() is not None: + raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}") + if ( + not monitor.latest_status + and now - launched_at > STATUS_TIMEOUT_SECONDS + ): + raise RuntimeError("CAM-STATUS-202:no calibration status received") + if ( + monitor.latest_status + and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS + ): + raise RuntimeError("MOTION-COMM-303:calibration status stopped") + except KeyboardInterrupt as error: + latest_status["state"] = "ABORTED" + latest_status["reason"] = "operator_abort" + _request_safe_abort(monitor) + _write_trace(log_path, error) + exit_code = EXIT_SAFETY + except BaseException as error: + latest_status["state"] = "PAUSED" + latest_status["reason"] = str(error) + _write_trace(log_path, error) + exit_code = EXIT_QUALITY + finally: + if process is not None: + _stop_stack(process) + monitor.destroy_node() + log_stream.flush() + os.fsync(log_stream.fileno()) + log_stream.close() + + if exit_code != EXIT_PASS: + _, block = build_failure_report( + config, + session, + latest_status, + reason=str(latest_status.get("reason", "unknown_failure")), + ) + print(block, flush=True) + return latest_status, exit_code + + +def _startup_failure_block(path: Path, error: BaseException) -> str: + return "\n".join( + [ + "========== 请复制以下内容给开发者 ==========", + "结果：FAIL", + "错误代码：CFG-PRODUCT-001", + "失败阶段：启动静态预检", + f"问题：{error}", + f"产品配置：{path}", + "自动处理：未启动相机、SDK或机械手运动", + "建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。", + "========== 复制结束 ==========", + ] + ) + + +def _automatic_resume_candidate(config: ProductConfig) -> Path | None: + """Return the newest compatible failed attempt, never a passed session. + + Do not trust only ``latest_attempt``. A process interrupted during the + device-only startup gate may have already moved that pointer while still + containing no ``session_start`` checkpoint. In that case walk backwards + to the preceding usable failed session instead of throwing away hours of + completed tasks. + """ + root = config.session_root + try: + resolved_root = root.resolve(strict=True) + except OSError: + return None + + candidates: list[Path] = [] + pointer = config.session_root / "latest_attempt" + if pointer.exists(): + try: + candidates.append(pointer.resolve(strict=True)) + except OSError: + pass + try: + candidates.extend( + sorted( + ( + path + for path in root.iterdir() + if path.is_dir() and not path.name.startswith("latest_") + ), + key=lambda path: path.name, + reverse=True, + ) + ) + except OSError: + return None + + passed_pointer = config.session_root / "latest_passed" + passed: Path | None = None + if passed_pointer.exists(): + try: + passed = passed_pointer.resolve(strict=True) + except OSError: + pass + + seen: set[Path] = set() + for unresolved in candidates: + try: + candidate = unresolved.resolve(strict=True) + except OSError: + continue + if candidate in seen: + continue + seen.add(candidate) + if candidate.parent != resolved_root or not candidate.is_dir(): + continue + # A failed attempt older than the current formal release is stale and + # must not seed a new independent calibration. + if passed is not None and candidate.name <= passed.name: + continue + raw_path = candidate / "raw_samples.jsonl" + if not raw_path.is_file(): + continue + summary_path = candidate / "calibration_summary_zh.json" + summary: dict[str, Any] | None = None + if summary_path.is_file(): + try: + loaded = json.loads(summary_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError): + continue + if not isinstance(loaded, dict) or loaded.get("result") != "FAIL": + continue + summary = loaded + hashes = summary.get("hashes", {}) + if not isinstance(hashes, Mapping): + continue + if ( + str(hashes.get("source_urdf_sha256", "")) + != config.source_urdf_sha256 + or str(hashes.get("camera_extrinsics_sha256", "")) + != config.camera_extrinsics_sha256 + ): + continue + start: dict[str, Any] | None = None + try: + with raw_path.open("r", encoding="utf-8") as stream: + for line in stream: + if not line.strip(): + continue + value = json.loads(line) + if ( + isinstance(value, dict) + and value.get("kind") == "session_start" + ): + start = value + break + except (OSError, json.JSONDecodeError): + continue + if ( + start is None + or start.get("hand_type") != "right" + or start.get("tag_layout") != "g20_right_15" + or start.get("source_urdf_sha256") + != config.source_urdf_sha256 + ): + continue + if summary is None: + # Ctrl+C can terminate the ROS launch tree before the wrapper gets + # a chance to create calibration_summary_zh.json. The immutable + # checkpoint itself is enough to resume only after independently + # proving that its external geometry still matches the product. + try: + checkpoint_extrinsics = Path( + str(start["camera_extrinsics_file"]) + ).expanduser().resolve(strict=True) + if sha256_file(checkpoint_extrinsics) != ( + config.camera_extrinsics_sha256 + ): + continue + except (KeyError, OSError, ValueError): + continue + return candidate + return None + + +def run( + config_path: str | Path, + *, + workspace: str | Path | None = None, + preflight_only: bool = False, + allow_resume: bool = True, +) -> int: + path = Path(config_path).expanduser().resolve() + try: + # Resolve every file and camera identity before allowing a hardware + # process to start. A second load enables the real CAN existence gate. + config = load_product_config(path, workspace=workspace, check_can=False) + load_product_config(path, workspace=workspace, check_can=True) + except BaseException as error: + print(_startup_failure_block(path, error), flush=True) + return EXIT_QUALITY + if preflight_only: + print("PASS：产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。") + return EXIT_PASS + + config.session_root.mkdir(parents=True, exist_ok=True) + resume_candidate = ( + _automatic_resume_candidate(config) if allow_resume else None + ) + if resume_candidate is not None: + print( + "检测到兼容的失败会话，将恢复已完整通过的关节任务：" + f"{resume_candidate.name}。失败中的当前任务会从头重做。", + flush=True, + ) + maximum_sessions = config.required_independent_passes + for pass_index in range(maximum_sessions): + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + while session.exists(): + time.sleep(1.0) + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + if maximum_sessions > 1: + print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True) + status, code = _run_hardware_session( + config, + session, + resume_from=(resume_candidate if pass_index == 0 else None), + ) + if code != EXIT_PASS: + return code + try: + summary, release_ready = finalize_session_artifacts( + config, session, node_status=status + ) + except BaseException as error: + _write_trace(session / "calibration.log", error) + status = dict(status) + status["state"] = "PAUSED" + status["reason"] = f"PUB-ARTIFACT-601:{error}" + _, block = build_failure_report(config, session, status, reason=status["reason"]) + print(block, flush=True) + return EXIT_QUALITY + if release_ready: + print( + "\n".join( + [ + "PASS：G20右手标定、URDF修正和独立复验全部通过。", + f"正式结果：{config.session_root / 'latest_passed'}", + f"JSON：{session / f'g20_right_{config.serial_number}_calibration.json'}", + f"URDF：{summary['artifacts']['urdf']}", + ] + ), + flush=True, + ) + return EXIT_PASS + print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True) + return EXIT_QUALITY + + +def main(args: list[str] | None = None) -> None: + parser = argparse.ArgumentParser(description="G20右手一键精密标定") + parser.add_argument("--config", default=str(_default_product_config())) + parser.add_argument("--workspace", default=None) + parser.add_argument("--preflight-only", action="store_true") + parser.add_argument( + "--no-resume", + action="store_true", + help="忽略失败会话，从第一个关节开始全新采集", + ) + arguments = parser.parse_args(args) + configure_fastdds_large_image_transport() + ros_log_dir = Path( + os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs") + ) + ros_log_dir.mkdir(parents=True, exist_ok=True) + rclpy.init() + try: + code = run( + arguments.config, + workspace=arguments.workspace, + preflight_only=arguments.preflight_only, + allow_resume=not arguments.no_resume, + ) + finally: + if rclpy.ok(): + rclpy.shutdown() + raise SystemExit(code) + + +if __name__ == "__main__": + main() 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 new file mode 100644 index 0000000..13f6ee9 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py @@ -0,0 +1,394 @@ +"""Stable Chinese progress and copy/paste diagnostics for non-expert users.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +import math +from pathlib import Path +import time +from typing import Any, Mapping + +from .product import ProductConfig +from .storage import atomic_write_json +from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh + + +def _duration(seconds: float | None) -> str: + if seconds is None or not math.isfinite(seconds) or seconds < 0.0: + return "计算中" + value = int(round(seconds)) + return f"{value // 60}分{value % 60:02d}秒" + + +def _task_tag_id_status(views: Mapping[str, Any]) -> str: + """Render every currently required Tag ID with its live visibility.""" + labels = {"front": "正面", "side": "侧面", "top": "顶部"} + ordered_views = [ + *[name for name in ("front", "side", "top") if name in views], + *sorted(name for name in views if name not in labels), + ] + groups: list[str] = [] + for name in ordered_views: + item = views.get(name, {}) + if not isinstance(item, Mapping): + continue + required = sorted({int(value) for value in item.get("required_tag_ids", [])}) + if not required: + continue + visible = {int(value) for value in item.get("detected_tag_ids", [])} + locked = { + int(value) + for value in item.get("locked_reference_tag_ids", []) + } + ids = ",".join( + f"{tag_id}{'锁' if tag_id in locked else '✓' if tag_id in visible else '✗'}" + for tag_id in required + ) + groups.append(f"{labels.get(name, name)}[{ids}]") + if not groups: + return "" + return ( + " 当前任务ID：" + + " ".join(groups) + + "（✓实时可见/锁=基准锁定/✗不可用）" + ) + + +@dataclass +class ProgressEstimator: + started_at: float + last_progress: float = 0.0 + last_progress_at: float = 0.0 + seconds_per_fraction: float | None = None + + @classmethod + def start(cls) -> "ProgressEstimator": + now = time.monotonic() + return cls(started_at=now, last_progress_at=now) + + def remaining(self, progress: float, now: float | None = None) -> float | None: + current = time.monotonic() if now is None else float(now) + value = float(max(0.0, min(1.0, progress))) + delta = value - self.last_progress + elapsed = current - self.last_progress_at + if delta >= 0.002 and elapsed > 0.0: + estimate = elapsed / delta + self.seconds_per_fraction = ( + estimate + if self.seconds_per_fraction is None + else 0.8 * self.seconds_per_fraction + 0.2 * estimate + ) + self.last_progress = value + self.last_progress_at = current + if value >= 1.0: + return 0.0 + if self.seconds_per_fraction is not None: + return max(0.0, self.seconds_per_fraction * (1.0 - value)) + if value >= 0.02: + return max(0.0, (current - self.started_at) * (1.0 - value) / value) + return None + + +def render_progress_zh( + serial_number: str, + status: Mapping[str, Any], + estimator: ProgressEstimator, +) -> str: + progress = float(status.get("progress", 0.0)) + active = status.get("active", {}) + if not isinstance(active, Mapping): + active = {} + state = str(status.get("state", "PREFLIGHT")) + stage = STATE_NAMES_ZH.get(state, state) + preflight_mode = str(status.get("preflight_mode", "")) + if preflight_mode == "device_only_before_baseline": + stage = "设备连接预检" + elif preflight_mode == "recovering_baseline": + stage = "安全恢复基准形态" + elif preflight_mode == "baseline_tags_after_recovery": + stage = "基准姿态标签预检" + elif str(status.get("reason", "")) == ( + "holding_same_finger_clearance_before_next_task" + ): + stage = "保持避让姿态，切换同指下一项" + elif str(status.get("reason", "")) == ( + "waiting_for_task_tags_at_sweep_start" + ): + stage = "已到扫描起点，等待任务Tag" + joints = active.get("joints", []) + joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints) + if str(active.get("task_name", "")).endswith("_roll_multiview") and joints: + joint = ( + JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0])) + + "（正面+侧面同步）" + ) + if not joint: + joint = str(active.get("label_zh", "")) or ( + "等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手" + ) + direction = { + "decreasing": "递减", + "increasing": "递增", + }.get(str(active.get("direction", "")), "-") + cycle = active.get("cycle", "-") + repetitions = active.get("repetitions", 4) + requested = active.get( + "current_motion_target_u8", + active.get("command_u8", active.get("pose_name", "-")), + ) + actual = active.get("actual_u8", "-") + views = status.get("views", {}) + if not isinstance(views, Mapping): + views = {} + ready_cameras = sum( + bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid")) + for item in views.values() + if isinstance(item, Mapping) + ) + required_tags = sum( + len(item.get("required_tag_ids", [])) + for item in views.values() + if isinstance(item, Mapping) + ) + visible_tags = sum( + len( + { + *item.get("detected_tag_ids", []), + *item.get("locked_reference_tag_ids", []), + } + ) + for item in views.values() + if isinstance(item, Mapping) + ) + locked_tag_count = sum( + len(item.get("locked_reference_tag_ids", [])) + for item in views.values() + if isinstance(item, Mapping) + ) + configured_tags = sum( + len(item.get("configured_tag_ids", item.get("required_tag_ids", []))) + for item in views.values() + if isinstance(item, Mapping) + ) + visible_configured_tags = sum( + len( + item.get( + "visible_configured_tag_ids", + item.get("detected_tag_ids", []), + ) + ) + for item in views.values() + if isinstance(item, Mapping) + ) + occlusion_allowed = bool( + state in {"PREFLIGHT", "WAIT_START"} + or active.get("pose_name") + ) + if preflight_mode == "device_only_before_baseline": + tag_status = ( + f"当前 {visible_configured_tags}/{configured_tags}" + "（基准恢复后检查）" + ) + elif preflight_mode == "recovering_baseline": + tag_status = ( + f"当前 {visible_configured_tags}/{configured_tags}" + "（恢复中不作为门槛）" + ) + elif occlusion_allowed: + tag_status = ( + f"当前 {visible_configured_tags}/{configured_tags}（允许遮挡） " + f"本阶段必需 {visible_tags}/{required_tags}" + ) + else: + tag_status = ( + f"{visible_tags}/{required_tags} 有效" + + (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "") + ) + task_tag_ids = ( + _task_tag_id_status(views) + if state + in { + "PREPARE_SWEEP", + "SWEEP", + "VALIDATION_MOVE", + "VALIDATION_CAPTURE", + } + else "" + ) + retry = int(active.get("automatic_retry_count", 0)) + fit_attempt = int(active.get("fit_attempt", 1)) + fit_attempt_limit = int(active.get("fit_attempt_limit", 1)) + feedback_hz = float(status.get("feedback_hz", 0.0)) + eta = ( + "等待Tag" + if str(status.get("reason", "")) + == "waiting_for_task_tags_at_sweep_start" + else _duration(estimator.remaining(progress)) + ) + lines = [ + f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}", + f"阶段：{stage}（第 {cycle}/{repetitions} 轮）", + f"任务：{joint} 命令/反馈：{requested}/{actual} 方向：{direction}", + 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: + lines.append( + f"拟合：整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试；" + "每次包含完整四轮双向扫描" + ) + resume = status.get("resume", {}) + if isinstance(resume, Mapping) and resume.get("used"): + lines.append( + "断点：已恢复 " + f"{int(resume.get('completed_task_count', 0))}/" + f"{int(resume.get('total_task_count', 16))} 个完整任务；" + "失败任务已丢弃并重新采集" + ) + return "\n".join(lines) + + +def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]: + value = str(reason) + active = status.get("active", {}) + if not isinstance(active, Mapping): + active = {} + views = status.get("views", {}) + missing = [ + tag + for item in views.values() + if isinstance(item, Mapping) + for tag in item.get("missing_tag_ids", []) + ] if isinstance(views, Mapping) else [] + group_pnp_reasons = [ + str(item.get("group_pnp_reason")) + for item in views.values() + if isinstance(item, Mapping) and item.get("group_pnp_reason") + ] if isinstance(views, Mapping) else [] + if value.startswith("CFG-"): + return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。" + if "motor_state_stalled" in value: + return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。" + if any( + token in value + for token in ( + "sweep_missing_endpoint_bin", + "sweep_bins_too_few", + "sweep_bin_gap_too_large", + "task_precheck_missing_command_127", + ) + ): + return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。" + if "synchronised" in value and not missing and group_pnp_reasons: + return "CAM-GEOMETRY-201", "Tag可见，但整组PnP候选持续被几何检查拒绝", "不要调整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): + return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。" + if "camera" in value or "pnp" in value or "extrinsics" in value: + return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动，然后复制本诊断块给开发者。" + if "checkpoint" in value: + return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。" + if "fit" in value or "trajectory" in value or "axis" in value: + return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。" + 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: + return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖；复制本诊断块给开发者。" + return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者，完整堆栈已写入calibration.log。" + + +def build_failure_report( + config: ProductConfig, + session: str | Path, + status: Mapping[str, Any], + *, + reason: str, +) -> tuple[dict[str, Any], str]: + directory = Path(session).resolve() + code, problem, suggestion = classify_error(reason, status) + active = status.get("active", {}) + if not isinstance(active, Mapping): + active = {} + else: + active = dict(active) + views = status.get("views", {}) + if isinstance(views, Mapping): + group_pnp_reasons = { + str(view): str(item.get("group_pnp_reason")) + for view, item in views.items() + if isinstance(item, Mapping) and item.get("group_pnp_reason") + } + if group_pnp_reasons: + active["group_pnp_reasons"] = group_pnp_reasons + try: + explanation, automatic_action = three_camera_reason_zh( + str(status.get("state", "")), reason, active + ) + except Exception: + explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据" + camera_state = { + view: ( + "OK" + if isinstance(item, Mapping) + and item.get("camera_info_valid", item.get("ready")) + and item.get("camera_extrinsics_valid", item.get("ready")) + and item.get("stream_alive", item.get("ready")) + else "NOT_READY" + ) + for view, item in (views.items() if isinstance(views, Mapping) else []) + } + session_id = f"{config.serial_number}_{directory.name}" + metrics = { + "valid_frames": active.get("valid_frames"), + "sample": active.get("sample", {}), + "automatic_retry_count": active.get("automatic_retry_count", 0), + } + payload: dict[str, Any] = { + "schema_version": 1, + "serial_number": config.serial_number, + "session_id": session_id, + "result": "FAIL", + "error_code": code, + "stage": str(status.get("state", "startup")), + "reason": str(reason), + "problem_zh": problem, + "explanation_zh": explanation, + "automatic_action_zh": automatic_action, + "suggestion_zh": suggestion, + "metrics": metrics, + "camera_state": camera_state, + "feedback_hz": status.get("feedback_hz", 0.0), + "hashes": { + "product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(), + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "source_urdf_sha256": config.source_urdf_sha256, + }, + "session_dir": str(directory), + "quality": {"passed": False}, + } + atomic_write_json(directory / "calibration_summary_zh.json", payload) + block = "\n".join( + [ + "========== 请复制以下内容给开发者 ==========", + f"会话编号：{session_id}", + "结果：FAIL", + f"错误代码：{code}", + f"失败阶段：{payload['stage']}", + f"问题：{problem}", + f"详细说明：{explanation}", + f"自动处理：{automatic_action}", + f"关键指标：{json.dumps(metrics, 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']}", + f"外参哈希：{config.camera_extrinsics_sha256}", + f"源 URDF 哈希：{config.source_urdf_sha256}", + f"会话目录：{directory}", + f"建议：{suggestion}", + "========== 复制结束 ==========", + ] + ) + return payload, block 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 4389939..b1b42e0 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 @@ -477,6 +477,11 @@ def select_static_rigid_group_initialization( relative_translation_scale_m: float, normal_alignment_pairs: Sequence[tuple[str, str]] = (), normal_alignment_scale_rad: float = math.radians(5.0), + task_reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] | None = None, + task_reference_rotation_scale_rad: float = math.radians(1.0), + task_reference_translation_scale_m: float = 0.01, ) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: """Select a static multi-Tag IPPE branch path in bounded time. @@ -497,13 +502,18 @@ def select_static_rigid_group_initialization( normal_pair_names = tuple( (str(first), str(second)) for first, second in normal_alignment_pairs ) + task_references = dict(task_reference_pairs or {}) if not frames: raise ValueError("at least one PnP frame is required") if not role_names or len(set(role_names)) != len(role_names): raise ValueError("roles must be non-empty and unique") if any( parent not in role_names or child not in role_names - for parent, child in (*pair_names, *normal_pair_names) + for parent, child in ( + *pair_names, + *normal_pair_names, + *task_references, + ) ): raise ValueError("geometry pairs must reference roles") scales = ( @@ -513,6 +523,8 @@ def select_static_rigid_group_initialization( float(relative_rotation_scale_rad), float(relative_translation_scale_m), float(normal_alignment_scale_rad), + float(task_reference_rotation_scale_rad), + float(task_reference_translation_scale_m), ) if min(scales) <= 0.0: raise ValueError("static initialization scales must be positive") @@ -523,6 +535,8 @@ def select_static_rigid_group_initialization( relative_rotation_scale, relative_translation_scale, normal_scale, + task_reference_rotation_scale, + task_reference_translation_scale, ) = scales combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] @@ -578,6 +592,16 @@ def select_static_rigid_group_initialization( + float(np.linalg.norm(translation - reference_translation)) / relative_translation_scale ) + for pair, (task_rotation, task_translation) in task_references.items(): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((task_rotation.inv() * rotation).magnitude()) + / task_reference_rotation_scale + + float(np.linalg.norm(translation - task_translation)) + / task_reference_translation_scale + ) return float(score) best_total = float("inf") @@ -628,6 +652,9 @@ def select_static_rigid_group_initialization( quality = dict(quality) quality["total_cost"] = float(best_total) quality["initialization_search"] = "static_reference" + quality["task_reference_used"] = ( + "true" if task_references else "false" + ) return selected_path, quality @@ -1013,6 +1040,13 @@ class SquareTagGroupPoseTracker: normal_alignment_pairs: Sequence[tuple[str, str]] = (), normal_alignment_scale_rad: float = math.radians(5.0), maximum_normal_alignment_rad: float | None = None, + return_reference_rotation_scale_rad: float = math.radians(1.0), + return_reference_maximum_command_gap_u8: int = 8, + coupled_rotation_pairs: Sequence[ + tuple[str, str, str, str, float] + ] = (), + coupled_rotation_scale_rad: float = math.radians(3.0), + maximum_coupled_rotation_residual_rad: float | None = None, ) -> None: self.roles = tuple(str(role) for role in roles) self.adjacent_pairs = tuple( @@ -1023,6 +1057,22 @@ class SquareTagGroupPoseTracker: (str(first), str(second)) for first, second in normal_alignment_pairs ) + self.coupled_rotation_pairs = tuple( + ( + str(driver_parent), + str(driver_child), + str(follower_parent), + str(follower_child), + float(multiplier), + ) + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in coupled_rotation_pairs + ) if not self.roles or len(set(self.roles)) != len(self.roles): raise ValueError("roles must be non-empty and unique") if any( @@ -1054,6 +1104,20 @@ class SquareTagGroupPoseTracker: if maximum_normal_alignment_rad is None else float(maximum_normal_alignment_rad) ) + self.return_reference_rotation_scale_rad = float( + return_reference_rotation_scale_rad + ) + self.return_reference_maximum_command_gap_u8 = int( + return_reference_maximum_command_gap_u8 + ) + self.coupled_rotation_scale_rad = float( + coupled_rotation_scale_rad + ) + self.maximum_coupled_rotation_residual_rad = ( + None + if maximum_coupled_rotation_residual_rad is None + else float(maximum_coupled_rotation_residual_rad) + ) reset_seconds = float(reset_after_seconds) if min( self.maximum_pose_jump_rad, @@ -1062,6 +1126,8 @@ class SquareTagGroupPoseTracker: self.relative_translation_scale_m, self.reprojection_scale_px, self.normal_alignment_scale_rad, + self.return_reference_rotation_scale_rad, + self.coupled_rotation_scale_rad, reset_seconds, ) <= 0.0: raise ValueError("group tracking scales must be positive") @@ -1069,11 +1135,33 @@ class SquareTagGroupPoseTracker: raise ValueError("reprojection_weight must be non-negative") if self.initialization_frames < 1: raise ValueError("initialization_frames must be positive") + if self.return_reference_maximum_command_gap_u8 < 0: + raise ValueError( + "return reference maximum command gap must be non-negative" + ) if ( self.maximum_normal_alignment_rad is not None and self.maximum_normal_alignment_rad <= 0.0 ): raise ValueError("maximum normal alignment must be positive") + if any( + role not in self.roles + for coupling in self.coupled_rotation_pairs + for role in coupling[:4] + ): + raise ValueError("coupled rotation pairs must reference roles") + if any( + multiplier <= 0.0 + for *_, multiplier in self.coupled_rotation_pairs + ): + raise ValueError("coupled rotation multipliers must be positive") + if ( + self.maximum_coupled_rotation_residual_rad is not None + and self.maximum_coupled_rotation_residual_rad <= 0.0 + ): + raise ValueError( + "maximum coupled rotation residual must be positive" + ) self.reset_after_ns = int(reset_seconds * 1_000_000_000) self._previous: dict[str, SquareTagPose] = {} self._previous_stamp_ns: int | None = None @@ -1083,22 +1171,200 @@ class SquareTagGroupPoseTracker: self._initial_stamps_ns: list[int] = [] self.last_initialization_quality: dict[str, float | str] = {} self.branch_correction_counts: dict[str, int] = {} + self._decreasing_relative_rotations: dict[ + int, dict[tuple[str, str], Rotation] + ] = {} + self._coupled_reference_rotations: dict[ + tuple[str, str], Rotation + ] = {} + self._task_reference_relative_poses: dict[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] = {} - def reset(self) -> None: + def reset(self, *, preserve_task_reference: bool = False) -> None: self._previous.clear() self._previous_stamp_ns = None self._initial_candidates.clear() self._initial_stamps_ns.clear() self.last_initialization_quality.clear() self.branch_correction_counts.clear() + self._decreasing_relative_rotations.clear() + self._coupled_reference_rotations.clear() + if not preserve_task_reference: + self._task_reference_relative_poses.clear() + + def _task_reference_cost( + self, combination: Mapping[str, SquareTagPose] + ) -> float: + residual = 0.0 + for pair, (expected_rotation, expected_translation) in ( + self._task_reference_relative_poses.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + residual += ( + float((expected_rotation.inv() * rotation).magnitude()) + / self.return_reference_rotation_scale_rad + + float(np.linalg.norm(translation - expected_translation)) + / self.relative_translation_scale_m + ) + return residual + + def _coupled_rotation_residuals( + self, combination: Mapping[str, SquareTagPose] + ) -> tuple[float, ...]: + if not self.coupled_rotation_pairs: + return () + residuals: list[float] = [] + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in self.coupled_rotation_pairs: + driver_pair = (driver_parent, driver_child) + follower_pair = (follower_parent, follower_child) + if ( + driver_pair not in self._coupled_reference_rotations + or follower_pair not in self._coupled_reference_rotations + ): + return () + driver_rotation = _relative_pose( + combination[driver_parent], combination[driver_child] + )[0] + follower_rotation = _relative_pose( + combination[follower_parent], combination[follower_child] + )[0] + driver_travel = ( + self._coupled_reference_rotations[driver_pair].inv() + * driver_rotation + ).magnitude() + follower_travel = ( + self._coupled_reference_rotations[follower_pair].inv() + * follower_rotation + ).magnitude() + residuals.append( + abs(float(follower_travel) - multiplier * float(driver_travel)) + ) + return tuple(residuals) + + def _informative_coupled_rotation_costs( + self, + combinations: Sequence[Mapping[str, SquareTagPose]], + ) -> tuple[float, ...]: + """Return branch costs only while the weak coupling prior is credible. + + The URDF mimic ratio is useful for distinguishing two planar-IPPE + branches, but it is not measurement truth for a passive joint. Once + every otherwise viable combination disagrees with that ratio, using + it would bias the measured curve (and previously rejected every + frame). In that case fall back to visual continuity for this frame. + """ + residuals = tuple( + self._coupled_rotation_residuals(combination) + for combination in combinations + ) + if not residuals or not any(residuals): + return tuple(0.0 for _ in combinations) + if ( + self.maximum_coupled_rotation_residual_rad is not None + and not any( + values + and max(values) + <= self.maximum_coupled_rotation_residual_rad + for values in residuals + ) + ): + return tuple(0.0 for _ in combinations) + return tuple( + sum(values) / self.coupled_rotation_scale_rad + for values in residuals + ) + + def _return_reference( + self, command_u8: int | None + ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]: + if command_u8 is None or not self._decreasing_relative_rotations: + return {} + command = int(command_u8) + nearest = min( + self._decreasing_relative_rotations, + key=lambda candidate: abs(candidate - command), + ) + if ( + abs(nearest - command) + > self.return_reference_maximum_command_gap_u8 + ): + return {} + references = self._decreasing_relative_rotations[nearest] + commands = sorted(self._decreasing_relative_rotations) + axes: dict[tuple[str, str], np.ndarray | None] = {} + for pair in self.adjacent_pairs: + endpoint_delta = ( + self._decreasing_relative_rotations[commands[-1]][pair].inv() + * self._decreasing_relative_rotations[commands[0]][pair] + ).as_rotvec() + norm = float(np.linalg.norm(endpoint_delta)) + axes[pair] = ( + None + if norm < math.radians(5.0) + else endpoint_delta / norm + ) + return { + pair: (rotation, axes[pair]) + for pair, rotation in references.items() + } + + def _return_reference_cost( + self, + combination: Mapping[str, SquareTagPose], + reference: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray | None] + ], + ) -> float: + residual = 0.0 + for pair, (expected, motion_axis) in reference.items(): + vector = ( + expected.inv() + * _relative_pose( + combination[pair[0]], combination[pair[1]] + )[0] + ).as_rotvec() + if motion_axis is not None: + # The outbound trajectory identifies the physical one-DOF + # motion axis. Do not penalize return travel along that axis: + # it may contain real mechanical hysteresis that calibration + # must measure. A planar-IPPE mirror branch appears primarily + # as a large orthogonal tilt and is rejected by this residual. + vector = vector - motion_axis * float(vector @ motion_axis) + residual += float(np.linalg.norm(vector)) + return residual / self.return_reference_rotation_scale_rad def select( self, candidates_by_role: Mapping[str, Sequence[SquareTagPose]], *, stamp_ns: int, + trajectory_command_u8: int | None = None, + trajectory_direction: str | None = None, ) -> tuple[dict[str, SquareTagPose] | None, str]: """Return one mutually consistent pose for every configured role.""" + direction = ( + None + if trajectory_direction is None + else str(trajectory_direction) + ) + if direction not in {None, "decreasing", "increasing"}: + raise ValueError( + "trajectory_direction must be decreasing or increasing" + ) + return_reference = ( + self._return_reference(trajectory_command_u8) + if direction == "increasing" + else {} + ) candidate_lists = [ tuple(candidates_by_role.get(role, ())) for role in self.roles @@ -1175,6 +1441,15 @@ class SquareTagGroupPoseTracker: normal_alignment_scale_rad=( self.normal_alignment_scale_rad ), + task_reference_pairs=( + self._task_reference_relative_poses + ), + task_reference_rotation_scale_rad=( + self.return_reference_rotation_scale_rad + ), + task_reference_translation_scale_m=( + self.relative_translation_scale_m + ), ) ) selected = selected_path[-1] @@ -1195,23 +1470,31 @@ class SquareTagGroupPoseTracker: ): return None, "group_normal_alignment" else: + coupling_costs = self._informative_coupled_rotation_costs( + combinations + ) selected = min( - combinations, - key=lambda combination: ( + zip(combinations, coupling_costs), + key=lambda item: ( sum( pose.reprojection_error_px - for pose in combination.values() + for pose in item[0].values() ) / self.reprojection_scale_px + sum( _normal_alignment_rad( - combination[first], combination[second] + item[0][first], item[0][second] ) for first, second in self.normal_alignment_pairs ) / self.normal_alignment_scale_rad + + self._return_reference_cost( + item[0], return_reference + ) + + item[1] + + self._task_reference_cost(item[0]) ), - ) + )[0] maximum_alignment = max( ( _normal_alignment_rad( @@ -1238,7 +1521,7 @@ class SquareTagGroupPoseTracker: ) for pair in self.adjacent_pairs } - scored: list[tuple[float, dict[str, SquareTagPose]]] = [] + base_scored: list[tuple[float, dict[str, SquareTagPose]]] = [] for combination in combinations: absolute_rotation_motion = 0.0 absolute_translation_motion = 0.0 @@ -1305,11 +1588,23 @@ class SquareTagGroupPoseTracker: + relative_translation_motion / self.relative_translation_scale_m + self.reprojection_weight * reprojection_penalty + + self._return_reference_cost( + combination, return_reference + ) ) - scored.append((float(score), combination)) + base_scored.append((float(score), combination)) - if not scored: + if not base_scored: return None, "group_pose_jump" + coupling_costs = self._informative_coupled_rotation_costs( + [combination for _, combination in base_scored] + ) + scored = [ + (base_score + coupling_cost, combination) + for (base_score, combination), coupling_cost in zip( + base_scored, coupling_costs + ) + ] selected = min(scored, key=lambda item: item[0])[1] aligned: dict[str, SquareTagPose] = {} @@ -1343,4 +1638,39 @@ class SquareTagGroupPoseTracker: self._previous = aligned self._previous_stamp_ns = stamp + if ( + direction == "decreasing" + and trajectory_command_u8 is not None + and not self._coupled_reference_rotations + ): + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + _multiplier, + ) in self.coupled_rotation_pairs: + for pair in ( + (driver_parent, driver_child), + (follower_parent, follower_child), + ): + self._coupled_reference_rotations[pair] = _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + if direction == "decreasing" and trajectory_command_u8 is not None: + self._decreasing_relative_rotations[ + int(trajectory_command_u8) + ] = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + for pair in self.adjacent_pairs + } + if not self._task_reference_relative_poses: + self._task_reference_relative_poses = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + ) + for pair in self.adjacent_pairs + } return dict(aligned), "" 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 new file mode 100644 index 0000000..4a9f1f7 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py @@ -0,0 +1,237 @@ +"""Immutable G20 right-hand product identity and one-command preflight.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +from pathlib import Path +import re +from typing import Any, Mapping + +import yaml + +from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics + + +VIEWS = ("front", "side", "top") + + +def sha256_file(path: str | Path) -> str: + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _mapping(value: Any, name: str) -> Mapping[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"{name} must be a mapping") + return value + + +def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path: + text = str(value).strip() + if not text: + raise ValueError(f"{name} is required") + candidate = Path(text).expanduser() + if not candidate.is_absolute(): + candidate = workspace / candidate + return candidate.resolve() + + +def _camera_info_fingerprint(path: Path) -> str: + with path.open("r", encoding="utf-8") as stream: + payload = _mapping(yaml.safe_load(stream), str(path)) + + def values(key: str) -> list[float]: + item = _mapping(payload.get(key, {}), key) + return [float(value) for value in item.get("data", [])] + + return camera_info_fingerprint( + width=int(payload["image_width"]), + height=int(payload["image_height"]), + camera_matrix=values("camera_matrix"), + distortion=values("distortion_coefficients"), + rectification=values("rectification_matrix"), + projection=values("projection_matrix"), + ) + + +def _tag_ids(path: Path) -> set[int]: + with path.open("r", encoding="utf-8") as stream: + payload = _mapping(yaml.safe_load(stream), str(path)) + result: set[int] = set() + for node in payload.values(): + parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters") + tag = _mapping(parameters.get("tag"), "tag") + ids = [int(value) for value in tag.get("ids", [])] + if result.intersection(ids): + raise ValueError("Tag IDs must be unique across the three views") + result.update(ids) + return result + + +@dataclass(frozen=True) +class ProductConfig: + path: Path + workspace: Path + serial_number: str + can_interface: str + source_urdf: Path + source_urdf_sha256: str + camera_extrinsics: Path + camera_extrinsics_sha256: str + calibration_config: Path + calibration_config_sha256: str + tag_config: Path + tag_config_sha256: str + output_root: Path + cameras: Mapping[str, Mapping[str, str]] + required_independent_passes: int + static_repeatability_rad: float + + @property + def session_root(self) -> Path: + return self.output_root / self.serial_number + + +def load_product_config( + path: str | Path, + *, + workspace: str | Path | None = None, + check_can: bool = True, +) -> ProductConfig: + """Load and fully verify the fixed G20_RIGHT_001 product configuration.""" + source = Path(path).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"product config does not exist: {source}") + with source.open("r", encoding="utf-8") as stream: + raw = _mapping(yaml.safe_load(stream), str(source)) + if int(raw.get("schema_version", -1)) != 1: + raise ValueError("product config schema_version must be 1") + root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve() + 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") + can_interface = str(raw.get("can_interface", "")).strip() + if not can_interface: + raise ValueError("can_interface is required") + if check_can and not (Path("/sys/class/net") / can_interface).exists(): + raise ValueError(f"CAN interface does not exist: {can_interface}") + + artifacts = _mapping(raw.get("artifacts"), "artifacts") + source_urdf = _resolve_path( + artifacts.get("source_urdf"), workspace=root, name="source_urdf" + ) + extrinsics = _resolve_path( + artifacts.get("camera_extrinsics"), + workspace=root, + name="camera_extrinsics", + ) + calibration_config = _resolve_path( + artifacts.get("calibration_config"), + workspace=root, + name="calibration_config", + ) + tag_config = _resolve_path( + artifacts.get("tag_config"), workspace=root, name="tag_config" + ) + for name, candidate in ( + ("source URDF", source_urdf), + ("camera extrinsics", extrinsics), + ("calibration config", calibration_config), + ("Tag config", tag_config), + ): + if not candidate.is_file(): + raise ValueError(f"{name} does not exist: {candidate}") + + expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower() + expected_extrinsics_hash = str( + artifacts.get("camera_extrinsics_sha256", "") + ).lower() + expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower() + expected_calibration_hash = str( + artifacts.get("calibration_config_sha256", "") + ).lower() + hashes = ( + ("source URDF", source_urdf, expected_source_hash), + ("camera extrinsics", extrinsics, expected_extrinsics_hash), + ("Tag config", tag_config, expected_tag_hash), + ("calibration config", calibration_config, expected_calibration_hash), + ) + for name, candidate, expected in hashes: + if re.fullmatch(r"[0-9a-f]{64}", expected) is None: + raise ValueError(f"{name} expected SHA-256 is invalid") + actual = sha256_file(candidate) + if actual != expected: + raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}") + + expected_tag_ids = set(range(19)) - {7, 14, 16, 18} + if _tag_ids(tag_config) != expected_tag_ids: + raise ValueError( + "g20_right_15 Tag config must contain exactly IDs " + "0,1,2,3,4,5,6,8,9,10,11,12,13,15,17" + ) + + camera_raw = _mapping(raw.get("cameras"), "cameras") + if set(camera_raw) != set(VIEWS): + raise ValueError("cameras must contain front/side/top") + loaded_extrinsics = load_three_camera_extrinsics(extrinsics) + cameras: dict[str, dict[str, str]] = {} + serials: set[str] = set() + for view in VIEWS: + item = _mapping(camera_raw[view], f"cameras.{view}") + camera_serial = str(item.get("serial_number", "")) + info_path = _resolve_path( + item.get("camera_info"), workspace=root, name=f"{view}.camera_info" + ) + if not info_path.is_file(): + raise ValueError(f"{view} camera info does not exist: {info_path}") + if camera_serial in serials: + raise ValueError("the three camera serial numbers must be unique") + serials.add(camera_serial) + identity = loaded_extrinsics.cameras[view] + fingerprint = _camera_info_fingerprint(info_path) + if camera_serial != identity.serial_number: + raise ValueError(f"{view} camera serial does not match extrinsics") + if fingerprint != identity.intrinsics_sha256: + raise ValueError(f"{view} camera intrinsics do not match extrinsics") + cameras[view] = { + "serial_number": camera_serial, + "camera_info": str(info_path), + "camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")), + } + + output_root = _resolve_path( + raw.get("output_root", "calibration_output"), + workspace=root, + name="output_root", + ) + release = _mapping(raw.get("release", {}), "release") + passes = int(release.get("required_independent_passes", 2)) + if passes not in {1, 2}: + raise ValueError("required_independent_passes must be 1 or 2") + repeatability_deg = float(release.get("static_repeatability_deg", 1.0)) + if not 0.0 < repeatability_deg <= 5.0: + raise ValueError("static_repeatability_deg must be in (0, 5]") + return ProductConfig( + path=source, + workspace=root, + serial_number=serial, + can_interface=can_interface, + source_urdf=source_urdf, + source_urdf_sha256=expected_source_hash, + camera_extrinsics=extrinsics, + camera_extrinsics_sha256=expected_extrinsics_hash, + calibration_config=calibration_config, + calibration_config_sha256=expected_calibration_hash, + tag_config=tag_config, + tag_config_sha256=expected_tag_hash, + output_root=output_root, + cameras=cameras, + required_independent_passes=passes, + static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0, + ) 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 new file mode 100644 index 0000000..e6b7ac3 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py @@ -0,0 +1,569 @@ +"""Validate and atomically publish one inseparable G20 JSON/URDF session.""" + +from __future__ import annotations + +from datetime import datetime, timezone +import copy +import json +import math +import os +from pathlib import Path +import re +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .full_hand import ( + G20_COMBINATION_REQUIRED_TARGET_KEYS, + G20_RIGHT_19_LAYOUT, + MIMIC_DERIVED_FINGER_DIPS, + get_hand_calibration_profile, + validate_compact_payload, +) +from .product import ProductConfig, sha256_file +from .storage import atomic_write_json +from .urdf_zero import get_zero_calibration_profile + + +PASSIVE_JOINTS = frozenset( + {"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"} +) +ACTIVE_ZERO_JOINTS = frozenset( + get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints +) +RETAINED_ACTIVE_ZERO_JOINTS = frozenset( + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints +) - ACTIVE_ZERO_JOINTS + + +def _load_json(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + payload = json.load(stream) + if not isinstance(payload, dict): + raise ValueError(f"{path} must contain a JSON object") + return payload + + +def _joint_blocks(text: str) -> dict[str, str]: + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + return {match.group("name"): match.group(0) for match in pattern.finditer(text)} + + +def _mask_origin_rpy(block: str) -> str: + return re.sub( + r"(]*\brpy\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_RPY__\2", + block, + count=1, + ) + + +def _mask_active_origin_rpy_fields(text: str) -> str: + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + + def replace(match: re.Match[str]) -> str: + block = match.group(0) + if match.group("name") not in ACTIVE_ZERO_JOINTS: + return block + masked = _mask_origin_rpy(block) + if masked == block: + raise ValueError( + f"active joint {match.group('name')} has no origin.rpy" + ) + return masked + + return pattern.sub(replace, text) + + +def _triplet(value: str) -> np.ndarray: + result = np.asarray([float(item) for item in value.split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF triplet: {value}") + return result + + +def _joint_elements(path: str | Path) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in ET.parse(Path(path)).getroot().findall("joint") + } + + +def _verify_expected_origin_offsets( + source: str | Path, + corrected: str | Path, + expected_offsets_rad: Mapping[str, float], +) -> None: + offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()} + 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") + before = _joint_elements(source) + after = _joint_elements(corrected) + maximum_rotation_error = 0.0 + for name, original_joint in before.items(): + corrected_joint = after[name] + original_origin = original_joint.find("origin") + corrected_origin = corrected_joint.find("origin") + if original_origin is None or corrected_origin is None: + if original_origin is not corrected_origin: + raise ValueError(f"corrected URDF changed origin presence for {name}") + continue + original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) + corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) + if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0): + raise ValueError(f"corrected URDF changed origin.xyz for {name}") + original_rotation = Rotation.from_euler( + "xyz", _triplet(original_origin.get("rpy", "0 0 0")) + ) + corrected_rotation = Rotation.from_euler( + "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) + ) + expected_rotation = original_rotation + if name in offsets: + axis_node = original_joint.find("axis") + axis = _triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + norm = float(np.linalg.norm(axis)) + if norm <= 1.0e-12: + raise ValueError(f"source URDF joint {name} has a degenerate axis") + expected_rotation = expected_rotation * Rotation.from_rotvec( + axis / norm * offsets[name] + ) + error = float((expected_rotation.inv() * corrected_rotation).magnitude()) + maximum_rotation_error = max(maximum_rotation_error, error) + # Schema v4 intentionally stores zero offsets at eight decimal places. + # Older sessions wrote the corrected URDF from the full-precision solve, + # 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: + raise ValueError( + "corrected URDF origin.rpy does not match the published zero offsets" + ) + + +def verify_corrected_urdf( + source: str | Path, + corrected: str | Path, + *, + expected_offsets_rad: Mapping[str, float] | None = None, +) -> tuple[str, ...]: + """Prove that only active-joint origin.rpy attributes changed. + + Passive joint blocks, including their mimic elements, are compared as raw + UTF-8 text so formatting and numeric spelling are protected as well. + """ + source_text = Path(source).read_text(encoding="utf-8") + corrected_text = Path(corrected).read_text(encoding="utf-8") + before = _joint_blocks(source_text) + after = _joint_blocks(corrected_text) + if set(before) != set(after): + raise ValueError("corrected URDF changed the joint set") + # Text patchers may normalize only the final newline. It has no URDF/XML + # semantics; every joint block and every non-rpy byte remains guarded + # below. + if _mask_active_origin_rpy_fields(source_text).rstrip("\r\n") != ( + _mask_active_origin_rpy_fields(corrected_text).rstrip("\r\n") + ): + raise ValueError("corrected URDF changed content outside active origin.rpy") + changed: list[str] = [] + for name in before: + if before[name] == after[name]: + continue + if name not in ACTIVE_ZERO_JOINTS: + raise ValueError(f"corrected URDF changed protected joint {name}") + if _mask_origin_rpy(before[name]) != _mask_origin_rpy(after[name]): + raise ValueError(f"corrected URDF changed more than origin.rpy for {name}") + changed.append(name) + for name in PASSIVE_JOINTS: + if before.get(name) != after.get(name): + raise ValueError(f"passive joint/mimic block changed: {name}") + if expected_offsets_rad is not None: + _verify_expected_origin_offsets(source, corrected, expected_offsets_rad) + return tuple(sorted(changed)) + + +def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]: + """Return every session-local mesh after proving it resolves safely.""" + path = Path(urdf).expanduser().resolve() + root = ET.parse(path).getroot() + resources: dict[str, Path] = {} + for mesh in root.findall(".//mesh"): + filename = str(mesh.get("filename", "")).strip() + if not filename or "://" in filename or filename.startswith("package:"): + continue + relative = Path(filename) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError(f"URDF has an unsafe local mesh path: {filename}") + resolved = (path.parent / relative).resolve() + try: + resolved.relative_to(path.parent) + except ValueError as error: + raise ValueError(f"URDF mesh escapes the session: {filename}") from error + if not resolved.is_file(): + raise ValueError(f"URDF mesh resource is missing: {filename}") + resources[relative.as_posix()] = resolved + return dict(sorted(resources.items())) + + +def validate_runtime_curves_against_urdf_limits( + payload: Mapping[str, Any], source_urdf: str | Path +) -> None: + """Reject a runtime curve whose commanded q leaves a CAD safety limit.""" + validate_compact_payload(payload) + source_joints = _joint_elements(source_urdf) + for name, calibration in payload["joints"].items(): + joint = source_joints.get(str(name)) + if joint is None: + raise ValueError(f"source URDF is missing runtime 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") + 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") + 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"[{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 +) -> tuple[dict[str, Any], dict[str, int]]: + """Return a schema-preserving runtime payload bounded by CAD limits.""" + result = copy.deepcopy(dict(payload)) + validate_compact_payload(result) + source_joints = _joint_elements(source_urdf) + clipped_by_joint: dict[str, int] = {} + for name, calibration in result["joints"].items(): + joint = source_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") + 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") + source = np.asarray(calibration["angle_rad"], dtype=float) + bounded = np.clip(source, lower, upper) + count = int(np.count_nonzero(bounded != source)) + if count: + clipped_by_joint[str(name)] = count + calibration["angle_rad"] = [ + round(float(value), 8) for value in bounded + ] + validate_compact_payload(result) + return result, clipped_by_joint + + +def build_mujoco_validation_commands( + baseline: Sequence[int], +) -> dict[str, Any]: + if len(baseline) != 20: + raise ValueError("baseline must contain exactly 20 commands") + base = [int(value) for value in baseline] + if any(not 0 <= value <= 255 for value in base): + raise ValueError("baseline commands must be in [0, 255]") + + def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]: + values = list(base) + for index, value in changes.items(): + values[int(index)] = int(value) + return {"name": name, "label_zh": label, "command_u8": values} + + return { + "schema_version": 1, + "topic": "/g20/cb_right_hand_control_cmd", + "serial_number": "G20_RIGHT_001", + "poses": [ + pose("all_open", "全开", {}), + pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), + pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), + pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), + pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), + pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), + pose( + "half_grip", + "四指半握", + {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, + ), + pose( + "light_pinch", + "轻捏", + {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, + ), + ], + } + + +def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path: + parent = Path(root).resolve() + target = Path(session).resolve() + if target.parent != parent: + raise ValueError("session pointer target must be a direct child of the serial root") + if not target.is_dir(): + raise ValueError(f"session directory does not exist: {target}") + if name not in {"latest_attempt", "latest_passed"}: + raise ValueError("unsupported session pointer name") + parent.mkdir(parents=True, exist_ok=True) + destination = parent / name + temporary = parent / f".{name}.{os.getpid()}.tmp" + if temporary.is_symlink(): + temporary.unlink() + elif temporary.exists(): + raise ValueError(f"temporary pointer path is occupied: {temporary}") + os.symlink(target.name, temporary, target_is_directory=True) + os.replace(temporary, destination) + return destination + + +def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]: + validate_compact_payload(payload) + result = { + name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"]) + for name in ACTIVE_ZERO_JOINTS + } + if set(result) != ACTIVE_ZERO_JOINTS or any( + not math.isfinite(value) for value in result.values() + ): + raise ValueError("payload does not contain 12 finite observable zero offsets") + return result + + +def compare_session_offsets( + first: Mapping[str, Any], + second: Mapping[str, Any], + *, + maximum_difference_rad: float, +) -> dict[str, float]: + left = active_offsets(first) + right = active_offsets(second) + 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( + f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items() + ) + raise ValueError(f"independent-session static-zero mismatch: {details}") + return differences + + +def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]: + directory = Path(session).resolve() + urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf")) + if len(urdfs) != 1: + raise ValueError("session must contain exactly one corrected URDF") + return { + "json": directory / f"g20_right_{serial_number}_calibration.json", + "urdf": urdfs[0], + "summary": directory / "calibration_summary_zh.json", + "commands": directory / "mujoco_validation_commands.json", + "raw": directory / "raw_samples.jsonl", + "log": directory / "calibration.log", + } + + +def find_compatible_prior_session( + config: ProductConfig, + current: str | Path, + current_payload: Mapping[str, Any], +) -> tuple[Path | None, dict[str, float]]: + current_path = Path(current).resolve() + for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True): + if candidate.resolve() == current_path: + continue + json_path = candidate / f"g20_right_{config.serial_number}_calibration.json" + summary_path = candidate / "calibration_summary_zh.json" + if not json_path.is_file() or not summary_path.is_file(): + continue + try: + summary = _load_json(summary_path) + if not bool(summary.get("quality", {}).get("passed")): + continue + payload = _load_json(json_path) + differences = compare_session_offsets( + payload, + current_payload, + maximum_difference_rad=config.static_repeatability_rad, + ) + except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError): + continue + return candidate, differences + return None, {} + + +def _verify_combination_validation(combination: Mapping[str, Any]) -> None: + # The formal product uses the independently held-out fourth sweep cycle. + # The optional eight-pose check is a developer diagnostic because the + # 15-Tag layout has no DIP Tags and the axis-line zero solve does not + # establish an absolute Cartesian hand-base transform. When explicitly + # enabled, retain its strict coverage and error checks. + if not bool(combination.get("enabled")): + return + position_p95 = float(combination.get("position_p95_m", float("inf"))) + orientation_p95 = float( + combination.get("orientation_p95_rad", float("inf")) + ) + required = {str(value) for value in combination.get("required_targets", ())} + expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS) + observations = combination.get("observation_counts") + validations = combination.get("validation_counts") + if required != expected: + raise ValueError("combination validation has the wrong required-target set") + if not isinstance(observations, Mapping) or not isinstance(validations, Mapping): + raise ValueError("combination validation is missing per-target coverage") + missing_observations = sorted( + key for key in expected if int(observations.get(key, 0)) < 2 + ) + missing_validations = sorted( + key for key in expected if int(validations.get(key, 0)) < 1 + ) + if missing_observations or missing_validations: + raise ValueError( + "combination validation target coverage is incomplete: " + f"observations={','.join(missing_observations) or '-'}; " + f"validations={','.join(missing_validations) or '-'}" + ) + if ( + not bool(combination.get("completed")) + or int(combination.get("completed_poses", 0)) != 8 + or position_p95 > 0.003 + or orientation_p95 > math.radians(2.0) + ): + raise ValueError( + "eight-pose four-finger combination validation is incomplete or failed" + ) + + +def finalize_session_artifacts( + config: ProductConfig, + session: str | Path, + *, + node_status: Mapping[str, Any], +) -> tuple[dict[str, Any], bool]: + """Create diagnostics and update latest_passed only after every guard.""" + directory = Path(session).resolve() + paths = session_artifact_paths(directory, config.serial_number) + for name in ("json", "urdf", "raw", "log"): + if not paths[name].is_file(): + raise ValueError(f"session is missing {name}: {paths[name]}") + payload = _load_json(paths["json"]) + validate_compact_payload(payload) + if payload.get("schema_version") != 4 or payload.get("side") != "right": + raise ValueError("runtime JSON is not the compact right-hand schema v4") + if not bool(payload.get("quality", {}).get("passed")): + raise ValueError("runtime JSON quality is not passed") + payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( + payload, config.source_urdf + ) + if clipped_runtime_joints: + # A node from before runtime-limit saturation may already have written + # an otherwise valid commit candidate. Normalize it before hashing + # and publication so completed sessions can be recovered offline. + atomic_write_json(paths["json"], payload) + combination = node_status.get("combination_validation") + if not isinstance(combination, Mapping): + raise ValueError("node status is missing combination validation") + _verify_combination_validation(combination) + offsets = active_offsets(payload) + changed_joints = verify_corrected_urdf( + config.source_urdf, + paths["urdf"], + expected_offsets_rad=offsets, + ) + validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) + mesh_resources = verify_urdf_mesh_resources(paths["urdf"]) + mesh_hashes = { + name: sha256_file(path) for name, path in mesh_resources.items() + } + commands = build_mujoco_validation_commands(payload["baseline_command_u8"]) + atomic_write_json(paths["commands"], commands) + + prior, differences = find_compatible_prior_session(config, directory, payload) + release_ready = config.required_independent_passes == 1 or prior is not None + quality = dict(payload["quality"]) + summary: dict[str, Any] = { + "schema_version": 1, + "serial_number": config.serial_number, + "session_id": f"{config.serial_number}_{directory.name}", + "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION", + "quality": quality, + "runtime_limit_clipped_bins": clipped_runtime_joints, + "runtime_curve_domain": "requested_command_u8", + "static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS), + "retained_active_urdf_zero_joints": sorted( + RETAINED_ACTIVE_ZERO_JOINTS + ), + "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS), + "mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS), + "changed_urdf_joint_origins": list(changed_joints), + "hashes": { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "corrected_urdf_sha256": sha256_file(paths["urdf"]), + "mesh_resources_sha256": mesh_hashes, + "calibration_json_sha256": sha256_file(paths["json"]), + "raw_samples_sha256": sha256_file(paths["raw"]), + }, + "holdout": { + "training_cycles": [0, 1, 2], + "validation_cycle": 3, + "isolated": True, + }, + "combination_validation": dict(combination), + "formal_release": { + "passed": release_ready, + "required_independent_passes": config.required_independent_passes, + "comparison_session": None if prior is None else prior.name, + "maximum_static_difference_deg": ( + None if not differences else math.degrees(max(differences.values())) + ), + }, + "node_status": dict(node_status), + "created_at_utc": datetime.now(timezone.utc).isoformat(), + "artifacts": {name: path.name for name, path in paths.items()}, + } + atomic_write_json(paths["summary"], summary) + # Recompute hashes after all files exist and validate the pair once more + # immediately before the one atomic publication operation. + if sha256_file(config.source_urdf) != config.source_urdf_sha256: + raise ValueError("source URDF changed during calibration") + verify_corrected_urdf( + config.source_urdf, + paths["urdf"], + expected_offsets_rad=offsets, + ) + validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) + final_mesh_hashes = { + name: sha256_file(path) + for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() + } + if final_mesh_hashes != mesh_hashes: + raise ValueError("URDF mesh resources changed during publication") + if release_ready: + atomic_session_pointer(config.session_root, "latest_passed", directory) + return summary, release_ready 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 c648bf1..2cf5875 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 @@ -48,6 +48,10 @@ JOINT_NAMES_ZH = { "pinky_pip": "小指PIP", "pinky_dip": "小指DIP（被动）", "thumb_cmc_yaw": "拇指CMC侧摆", + "index_mcp_roll_side": "食指MCP侧摆（侧面校验）", + "middle_mcp_roll_side": "中指MCP侧摆（侧面校验）", + "ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）", + "pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）", } @@ -86,6 +90,12 @@ def _task_text(active: Mapping[str, Any]) -> str: f"{_format_u8(active.get('target_u8'))}、实际" f"{_format_u8(active.get('actual_u8'))}" ) + if active.get("kind") == "cross_view_roll_diagnostic": + return ( + f"{active.get('finger', '?')}侧摆跨机位诊断完成：" + f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，" + f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°" + ) if active.get("kind") == "validation": return ( f"{view}机位，随机复测，电机{active.get('motor_index')}，" @@ -132,10 +142,19 @@ def three_camera_reason_zh( context = fields[1] if len(fields) > 1 else "unknown" error_match = re.search(r"error_u8=([0-9.]+)", reason) error = error_match.group(1) if error_match else "未知" + timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason) + timeout_value = active.get("timeout_seconds") + if timeout_value is None and timeout_match is not None: + timeout_value = float(timeout_match.group(1)) + duration = ( + f"连续{float(timeout_value):g}秒" + if timeout_value is not None + else "在规定时间内" + ) motor = active.get("motor_index") if motor is not None: return ( - f"电机{motor}反馈连续8秒没有向目标推进；目标" + f"电机{motor}反馈{duration}没有向目标推进；目标" f"{_format_u8(active.get('target_u8'))}、实际" f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，" f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8" @@ -144,12 +163,29 @@ def three_camera_reason_zh( "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。", ) return ( - f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8" + f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8" f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。", "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的" "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。", ) + base_reason, separator, reason_detail = reason.partition(":") + if base_reason in { + "sweep_missing_endpoint_bin", + "sweep_bins_too_few", + "sweep_bin_gap_too_large", + "task_precheck_missing_command_127", + "task_precheck_detection_rate_too_low", + "synchronised_tag_state_timeout", + }: + reason = base_reason + detail_label = ( + JOINT_NAMES_ZH.get(reason_detail, reason_detail) + if separator and reason_detail + else "" + ) + detail_prefix = f"{detail_label}：" if detail_label else "" + if "URDF zero offset reached the configured" in reason: bound_match = re.search( r"configured\s+([0-9.]+)\s+degree bound", reason @@ -174,7 +210,7 @@ def three_camera_reason_zh( if reason == "sweep_missing_endpoint_bin": missing_text = "、".join(str(value) for value in missing) or "0或255" return ( - f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少" + f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少" f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为" f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到" "端点，但该时刻没有同时取得有效Tag图像和电机状态。", @@ -183,29 +219,64 @@ def three_camera_reason_zh( ) if reason == "sweep_bins_too_few": return ( - f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少" + f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少" f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。", "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。", ) if reason == "sweep_bin_gap_too_large": + gap_start = sample.get("maximum_bin_gap_start_u8") + gap_end = sample.get("maximum_bin_gap_end_u8") + gap_range = ( + "" + if gap_start is None or gap_end is None + else f"（{gap_start}→{gap_end}）" + ) return ( - f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，" - f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。", + f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，" + f"{gap_range}允许值不超过" + f"{sample.get('allowed_maximum_bin_gap', '?')}。", "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。", ) 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() + ) + return ( + f"{detail_prefix}Tag仍可见且反馈正常，但连续图像帧被整组PnP几何检查拒绝" + f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。", + "不要调整或反复粘贴Tag；保留当前会话并复制诊断块给开发者检查PnP分支逻辑。", + ) return ( - "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" + f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" "按时间戳配对”的图像帧。", "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，" "程序会重扫当前方向。", ) + if reason == "task_precheck_missing_command_127": + return ( + f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。", + "检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。", + ) + if reason == "task_precheck_detection_rate_too_low": + return ( + f"{detail_prefix}低速预检的有效Tag识别率低于门限。", + "检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。", + ) if reason == "sweep_start_position_timeout": return ( f"电机{active.get('motor_index')}未在规定时间到达扫描起点" f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。", "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。", ) + if reason == "sweep_start_tag_timeout": + return ( + "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的" + "同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。", + "只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。", + ) if reason == "sweep_timeout": return ( "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。", @@ -226,7 +297,7 @@ def three_camera_reason_zh( "随机复测位置没有采集到足够的同步有效Tag帧。", "检查当前机位Tag可见性后调用resume。", ) - if reason == "joint_fit_check_failed": + if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}: metric_names = { "plane_rms_mm": "平面拟合RMS", "radial_rms_mm": "圆半径拟合RMS", @@ -237,6 +308,9 @@ def three_camera_reason_zh( "arc_deg": "实测圆弧", "monotonic_correction_deg": "最大单调修正", "hysteresis_deg": "最大正反程差", + "baseline_hysteresis_deg": "baseline正反程关节角差", + "baseline_directional_gap_deg": "baseline方向分支间隙", + "baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差", "cycle_travel_range_deg": "三轮行程差", "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS", "axis_plane_rms_mm": "三维圆轴向RMS", @@ -259,6 +333,9 @@ def three_camera_reason_zh( "arc_deg": "°", "monotonic_correction_deg": "°", "hysteresis_deg": "°", + "baseline_hysteresis_deg": "°", + "baseline_directional_gap_deg": "°", + "baseline_directional_gap_range_deg": "°", "cycle_travel_range_deg": "°", "rotation_orthogonal_rms_deg": "°", "axis_plane_rms_mm": "mm", @@ -288,9 +365,14 @@ def three_camera_reason_zh( ) cycle_travel = failure.get("cycle_travel_deg", []) if cycle_travel: - detail += "（三轮=" + "/".join( + detail += "（各轮=" + "/".join( f"{float(value):.2f}°" for value in cycle_travel ) + "）" + cycle_values = failure.get("cycle_values_deg", []) + if cycle_values: + detail += "（各轮=" + "/".join( + f"{float(value):.2f}°" for value in cycle_values + ) + "）" details.append(detail) else: cycle = failure.get("cycle") @@ -300,11 +382,30 @@ def three_camera_reason_zh( f"{failure.get('reason', '未知原因')}" ) detail_text = "；".join(details) or "当前关节的轨迹拟合未通过" + directional_gap_failure = any( + str(failure.get("metric", "")).startswith( + "baseline_directional_gap" + ) + for failure in active.get("failures", []) + ) + if reason == "joint_fit_systematic_failure": + suggestion = ( + "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。" + "请直接复制诊断块给开发者，不要放宽门限。" + ) + else: + suggestion = ( + "方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，" + "处理后重新执行一键标定命令，程序会从最近可靠断点继续。" + if directional_gap_failure + else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；" + "程序只清除当前失败关节的数据并重扫" + f"{active.get('directions_to_rescan', 6)}个方向，" + "不需要手工调用ROS服务。" + ) return ( detail_text + "。程序已在当前关节结束后立即停止后续步骤。", - "修正Tag位置、遮挡或机械行程后调用" - "/g20_calibration/resume；程序只清除当前失败关节的数据" - f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。", + suggestion, ) if reason == "zero_model_validation_failed": reason_names = { @@ -345,11 +446,37 @@ def three_camera_reason_zh( "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；" "请检查Tag固定、相机外参和原始URDF后重新启动新标定。", ) + if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}: + return ( + "正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。", + "保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。", + ) if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}: return ( "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。", "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。", ) + if reason == "waiting_for_baseline_tags_after_recovery": + return ( + "机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。", + "若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。", + ) + if reason == "locking_fixed_base_references": + return ( + "基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。", + "无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。", + ) + if reason == "waiting_for_task_tags_at_sweep_start": + return ( + "电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag" + "允许被手指遮挡。", + "只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。", + ) + if reason == "call_start_for_baseline_recovery": + return ( + "相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。", + "保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。", + ) if reason == "call_start": return ( "三机位预检已经通过，等待操作员确认开始。", @@ -361,14 +488,62 @@ def three_camera_reason_zh( return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。" if reason == "collecting_timestamp_synchronised_tag_centres": return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。" + if reason == "collecting_dedicated_baseline_hold": + return ( + "正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。", + "无需操作，保持相机、标签和底座不动。", + ) + if reason == "steady checkpoint target is missing": + return ( + "首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。", + "程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。", + ) + if reason == "cross_view_roll_front_failure_deferred": + return ( + "正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。", + "无需操作；该诊断会锁定URDF发布。", + ) + if reason == "cross_view_roll_diagnostic_complete": + interpretation = str(active.get("interpretation", "")) + explanations = { + "both_views_confirm_direction_dependent_pose": ( + "正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。" + ), + "front_only_difference_check_roll_tag_bracket_or_front_pnp": ( + "只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。" + ), + "side_only_difference_check_side_tag_chain_or_side_pnp": ( + "只有侧面差异超限，优先检查侧面Tag链和侧面PnP。" + ), + "both_views_within_formal_hysteresis_limit": ( + "两个机位的静止回差均满足正式门限。" + ), + } + return ( + explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。") + + " 本次为诊断会话，不会生成或发布URDF。", + "保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。", + ) if reason == "capturing_random_validation_pose": return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。" if reason in {"calibration_passed", "calibration_complete"}: return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。" if reason == "quality_failed": return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。" + if reason == "combination_pose_prediction_failed": + return ( + "单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。", + "程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的" + "combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。", + ) if reason.startswith("prepare_") or state == "PREPARE_SWEEP": return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。" + if reason == "holding_same_finger_clearance_before_next_task": + return ( + "同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整" + "被测关节以衔接下一项。", + "无需操作，不要手动展开正在避让的手指。", + ) if state == "RETURN_BASELINE": return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。" if state == "FITTING": @@ -431,6 +606,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～" f"{_format_u8(sample.get('maximum_u8'))}" ) + detection_frames = int(active.get("detection_frames", 0)) + if detection_frames: + lines.append( + "本方向Tag检出：" + f"{float(active.get('detection_rate', 0.0)):.1%}" + f"（{active.get('detection_valid_frames', 0)}/" + f"{detection_frames}帧）" + ) auxiliary = active.get("auxiliary_motors", []) if auxiliary: lines.append( 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 ec0ea97..9cc4697 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 @@ -4,12 +4,14 @@ from __future__ import annotations from collections import deque from dataclasses import dataclass, field, replace +import hashlib import json import math from pathlib import Path import random import re import time +import traceback from typing import Any, Mapping, Sequence import numpy as np @@ -44,15 +46,26 @@ from .extrinsics import ( transform_matrix, ) from .full_hand import ( + G20_COMBINATION_REQUIRED_TARGET_KEYS, + G20_REFERENCE_THUMB_CMC_JOINTS, + G20_RIGHT_19_LAYOUT, LEFT_HAND_PROFILE, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, THREE_CAMERA_BASELINE_COMMAND, HandCalibrationProfile, JointCurveFit, SweepSpec, build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, build_calibration_speed_profile, build_compact_payload, calibration_auxiliary_commands, + canonical_zero_direction, + clamp_runtime_fits_to_urdf_limits, + compare_cross_view_roll_curves, + derive_mimic_passive_fits, + fit_joint_image_curve, get_hand_calibration_profile, ) from .hikrobot_camera import configure_fastdds_large_image_transport @@ -66,12 +79,15 @@ from .three_camera_diagnostics import render_three_camera_status_text_zh from .urdf_zero import ( LEFT_ZERO_PROFILE, JointAxisMeasurement, + UrdfKinematicModel, ZeroCalibrationProfile, ZeroSolveResult, + baseline_hysteresis_by_cycle_rad, + circle_direction_is_constrained, fit_joint_axis_measurement, fit_rotation_joint_curve, - measure_rotation_joint_observation, - rotation_curve_holdout_errors, + joint_curve_holdout_errors, + measure_joint_curve_observation, solve_urdf_zero_offsets, get_zero_calibration_profile, write_zero_corrected_urdf, @@ -90,6 +106,98 @@ STATE_PAUSED = "PAUSED" STATE_ABORTED = "ABORTED" STATE_COMPLETE = "COMPLETE" +STEADY_COMMAND_CHECKPOINTS: tuple[int, ...] = ( + 255, 224, 192, 160, 128, 96, 64, 32, 0 +) + +COMBINATION_TAG_TARGETS_BY_VIEW: Mapping[ + str, tuple[tuple[str, str], ...] +] = { + "front": tuple( + (name, name) + for name in ( + "thumb_cmc_pitch", "thumb_mcp", "thumb_ip", + "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", + ) + ), + "side": tuple( + (f"{finger}_{joint}", f"{finger}_{joint}") + for finger in ("index", "middle", "ring", "pinky") + for joint in ("pip", "dip") + ), + # The top-view Tag observes yaw but is mounted on the final CMC link, + # downstream of pitch in the URDF chain. + "top": (("thumb_cmc_yaw", "thumb_cmc_pitch"),), +} + +COMBINATION_BASE_OBSERVER_BY_VIEW = { + "front": "thumb_cmc_pitch", + "side": "index_pip", + "top": "thumb_cmc_yaw", +} + +COMBINATION_BASE_ROLE_BY_VIEW = { + "front": "front_base", + "side": "side_base", + "top": "top_base", +} + + +def combination_target_coverage( + observation_counts: Mapping[str, int], + validation_counts: Mapping[str, int], +) -> dict[str, Any]: + required = G20_COMBINATION_REQUIRED_TARGET_KEYS + observations = { + key: int(observation_counts.get(key, 0)) for key in required + } + validations = { + key: int(validation_counts.get(key, 0)) for key in required + } + missing_observations = sorted( + key for key, count in observations.items() if count < 2 + ) + missing_validations = sorted( + key for key, count in validations.items() if count < 1 + ) + return { + "required_targets": list(required), + "minimum_observations_per_target": 2, + "minimum_validations_per_target": 1, + "observation_counts": observations, + "validation_counts": validations, + "missing_observation_targets": missing_observations, + "missing_validation_targets": missing_validations, + "coverage_passed": not missing_observations and not missing_validations, + } + + +def _combination_observable_joints( + profile: HandCalibrationProfile, + view: str, + roles: Sequence[str], +) -> tuple[str, ...]: + """Return combination targets measurable from the currently visible Tags. + + Combination poses deliberately do not require all four adjacent fingers + to be visible at once. A joint is usable only when both Tags that define + its relative pose are present in the same camera frame. + """ + visible = {str(role) for role in roles} + targets = { + observation_name + for observation_name, _ in COMBINATION_TAG_TARGETS_BY_VIEW[view] + } + return tuple( + name + for name, spec in profile.record_specs.items() + if name in targets + and spec.measured + and spec.view == view + and spec.parent_role in visible + and spec.child_role in visible + ) + def _overall_progress( state: str, @@ -123,6 +231,658 @@ def _safe_name(value: str) -> str: return safe or "UNSET" +def _sweep_storage_key(spec: SweepSpec) -> str | int: + """Keep legacy motor-keyed retry state while separating product tasks.""" + return spec.key if spec.task_name else int(spec.motor_index) + + +def _sweep_views( + profile: HandCalibrationProfile, spec: SweepSpec +) -> tuple[str, ...]: + """Return every camera that contributes a joint to one physical sweep.""" + views: list[str] = [] + for joint_name in spec.joints: + joint = profile.record_specs[joint_name] + if joint.view is not None and joint.view not in views: + views.append(joint.view) + return tuple(views or [spec.view]) + + +def _node_profile(node: Any) -> HandCalibrationProfile: + return getattr(node, "profile", LEFT_HAND_PROFILE) + + +def _sweep_joints_for_view( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> tuple[str, ...]: + return tuple( + joint_name + for joint_name in spec.joints + if profile.record_specs[joint_name].view == str(view) + ) + + +def _fixed_base_role(view: str) -> str: + return { + "front": "front_base", + "side": "side_base", + "top": "top_base", + }[str(view)] + + +def _sweep_uses_locked_base_reference( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> bool: + """Allow the fixed front palm Tag to be hidden by finger clearance. + + Pinky/ring flexion is required to expose the middle/index side Tag, but + that same safe pose physically covers front Tag 0. The clearance remains + parked through that finger's roll, pitch and PIP tasks, including stages + where front is only an inactive status camera. Tag 0 is therefore locked + from baseline for every four-finger task; any moving Tag used by the + actual capture remains live and mandatory. + """ + return bool( + profile.layout_id == G20_RIGHT_19_LAYOUT + and str(view) == "front" + and spec.key.startswith(("pinky_", "ring_", "middle_", "index_")) + ) + + +def _selected_pose_qualities( + selected: Mapping[str, SquareTagPose], + live_qualities: Mapping[str, TagQuality], + *, + locked_base_role: str | None, + locked_base_quality: TagQuality | None, +) -> dict[str, TagQuality]: + """Attach PnP residuals without pretending a cached Tag is live. + + A hidden fixed palm Tag has no entry in ``live_qualities``. Its cached + pose can still be selected together with the live moving Tag, so quality + propagation must explicitly use the quality captured at baseline instead + of indexing a missing live detection. + """ + updated = dict(live_qualities) + for role, pose in selected.items(): + source = ( + locked_base_quality + if role == locked_base_role + else live_qualities.get(role) + ) + if source is None: + raise KeyError(f"selected role has no quality source: {role}") + updated[role] = TagQuality( + hamming=source.hamming, + decision_margin=source.decision_margin, + edge_pixels=source.edge_pixels, + reprojection_error_px=pose.reprojection_error_px, + ) + return updated + + +def _frames_for_joint( + frames: Sequence[Any], joint_name: str +) -> list[Any]: + """Filter an asynchronous multi-camera buffer to one joint observation.""" + return [ + frame + for frame in frames + if ( + not frame.joint_quaternions_xyzw + and not frame.joint_vectors_xyz_m + and not frame.image_vectors_xy_px + and not frame.parent_poses_common + and not frame.child_poses_common + ) + or ( + joint_name in frame.joint_quaternions_xyzw + and joint_name in frame.joint_vectors_xyz_m + and joint_name in frame.image_vectors_xy_px + and joint_name in frame.parent_poses_common + and joint_name in frame.child_poses_common + ) + ] + + +def _frames_cover_sweep_joints( + frames: Sequence[Any], spec: SweepSpec, minimum_per_joint: int +) -> bool: + minimum = max(1, int(minimum_per_joint)) + return all( + len(_frames_for_joint(frames, joint_name)) >= minimum + for joint_name in spec.joints + ) + + +def _frames_cover_sweep_motion( + frames: Sequence[Any], + spec: SweepSpec, + *, + motor_index: int, + minimum_per_joint: int, + minimum_span_u8: float, +) -> bool: + for joint_name in spec.joints: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < int(minimum_per_joint): + return False + values = [float(frame.state_u8[motor_index]) for frame in selected] + if max(values) - min(values) < float(minimum_span_u8): + return False + return True + + +def _joint_failure_reason( + reason: str, spec: SweepSpec, joint_name: str +) -> str: + return ( + str(reason) + if len(spec.joints) == 1 + else f"{reason}:{joint_name}" + ) + + +def _steady_checkpoint_commands( + profile: HandCalibrationProfile, item: "SweepItem" +) -> tuple[int, ...]: + """Return first-training-round steady commands after the start pose.""" + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + or item.precheck + or item.cycle != 0 + ): + return () + ordered = ( + STEADY_COMMAND_CHECKPOINTS + if item.direction == DIRECTION_DECREASING + else tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) + ) + if int(ordered[0]) != item.start_u8 or int(ordered[-1]) != item.target_u8: + raise RuntimeError("steady command checkpoint direction is inconsistent") + return tuple(int(value) for value in ordered[1:]) + + +RESUMABLE_SAMPLE_KINDS = frozenset( + {"sample", "baseline_hold_sample", "steady_command_sample"} +) + + +def _normalise_legacy_split_roll_resume_rows( + profile: HandCalibrationProfile, + rows: Sequence[Mapping[str, Any]], +) -> tuple[dict[str, Any], ...]: + """Map the former front/side roll tasks onto one multiview checkpoint. + + This lets a calibration that was already running during the task-layout + upgrade donate all fully completed data if it later needs to resume. The + two historical motions remain independent observations; they are never + presented as timestamp-synchronised frames or accepted unless both joint + record sets pass the normal completeness checks. + """ + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return tuple(dict(row) for row in rows) + aliases = { + f"{finger}_roll_{view}": f"{finger}_roll_multiview" + for finger in ("pinky", "ring", "middle", "index") + for view in ("front", "side") + } + normalised: list[dict[str, Any]] = [] + for source in rows: + row = dict(source) + old_task = str(row.get("task_name", "")) + replacement = aliases.get(old_task) + if ( + replacement is not None + and row.get("kind") != "synchronised_frame" + ): + row["task_name"] = replacement + row["resume_source_task_name"] = old_task + normalised.append(row) + return tuple(normalised) + + +def _latest_resume_rows( + rows: Sequence[Mapping[str, Any]], + *, + kind: str, + minimum_sweep_bins: int = 0, +) -> dict[tuple[str, str, int, str], list[dict[str, Any]]]: + """Select the latest *complete* persisted attempt for each group. + + An interrupted retry must not hide the previous complete scan. This is + especially important for the nine steady checkpoints, which are written + before the continuous trajectory starts. + """ + selected = [ + dict(row) + for row in rows + if row.get("kind") == kind + and row.get("task_name") + and row.get("joint") + and row.get("direction") in {DIRECTION_DECREASING, DIRECTION_INCREASING} + ] + grouped: dict[ + tuple[str, str, int, str], dict[int, list[dict[str, Any]]] + ] = {} + for row in selected: + key = ( + str(row["task_name"]), + str(row["joint"]), + int(row.get("cycle", -1)), + str(row["direction"]), + ) + grouped.setdefault(key, {}).setdefault( + int(row.get("attempt", 1)), [] + ).append(row) + result: dict[tuple[str, str, int, str], list[dict[str, Any]]] = {} + for key, attempts in grouped.items(): + for _attempt, candidate in sorted( + attempts.items(), reverse=True + ): + if kind == "steady_command_sample": + requested = { + int(row.get("requested_command_u8", -1)) + for row in candidate + } + complete = requested == set(STEADY_COMMAND_CHECKPOINTS) + elif kind == "sample": + feedback_bins = { + int(round(float(row.get("feedback_u8", -1)))) + for row in candidate + } + complete = ( + 0 in feedback_bins + and 255 in feedback_bins + and len(feedback_bins) >= int(minimum_sweep_bins) + ) + else: + complete = bool(candidate) + if complete: + result[key] = candidate + break + return result + + +def _unresolved_fit_failure_tasks( + profile: HandCalibrationProfile, + rows: Sequence[Mapping[str, Any]], +) -> set[str]: + """Return tasks whose newest complete acquisition was still rejected. + + A newer model may deliberately retire a metric for a constrained joint. + Such historical failures are safe to replay because the final fit will + evaluate the imported raw samples against all current quality gates. + """ + sample_attempts: dict[str, int] = {} + for row in rows: + task_name = str(row.get("task_name", "")) + if row.get("kind") == "sample" and task_name: + sample_attempts[task_name] = max( + sample_attempts.get(task_name, 0), + int(row.get("attempt", 1)), + ) + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) + unresolved: set[str] = set() + for row in rows: + if row.get("kind") != "fit_failure": + continue + task_name = str(row.get("task_name", "")) + if not task_name: + matches = [ + spec + for spec in profile.sweep_specs + if spec.view == str(row.get("view", "")) + and spec.motor_index == int(row.get("motor_index", -1)) + and set(spec.joints) + == {str(name) for name in row.get("joints", [])} + ] + if len(matches) != 1: + continue + task_name = matches[0].key + failure_attempt = int(row.get("attempt", 1)) + if failure_attempt < sample_attempts.get(task_name, 0): + continue + failures = [ + item + for item in row.get("failures", []) + if isinstance(item, Mapping) + ] + def retired(item: Mapping[str, Any]) -> bool: + metric = str(item.get("metric", "")) + joint_name = str(item.get("joint", "")) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric == "rotation_orthogonal_rms_deg" + and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + ): + return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric in { + "hysteresis_deg", + "command_direction_gap_deg", + } + ): + # Product curves now gate direction dependence with settled + # feedback-domain samples and final-cycle holdout. Historical + # failures that mixed velocity lag or firmware tracking + # deadband with mechanical hysteresis are safe to revalidate. + return True + return metric in { + "rotation_circle_axis_difference_deg", + "axis_plane_rms_mm", + } and circle_direction_is_constrained( + joint_name, + zero_profile.constrained_circle_joints, + ) + + retired_by_current_model = bool(failures) and all( + retired(item) for item in failures + ) + if not retired_by_current_model: + unresolved.add(task_name) + return unresolved + + +def resumable_completed_task_prefix( + profile: HandCalibrationProfile, + repetitions: int, + baseline: Sequence[int], + rows: Sequence[Mapping[str, Any]], + *, + minimum_sweep_bins: int = 32, + allow_sparse: bool = False, +) -> tuple[tuple[str, ...], tuple[dict[str, Any], ...]]: + """Return quality-preserving complete tasks and their raw records. + + A partially written task is never reused. Every joint, all four cycles, + both directions, both endpoints, the two nine-point command curves and + any required directional baseline holds must be present before the whole + task becomes a durable restart checkpoint. The compatibility default + returns only a contiguous prefix; product resume can opt into sparse + recovery so a failed early task does not discard independent later tasks. + """ + rows = _normalise_legacy_split_roll_resume_rows(profile, rows) + samples = _latest_resume_rows( + 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") + directions = (DIRECTION_DECREASING, DIRECTION_INCREASING) + expected_checkpoints = set(STEADY_COMMAND_CHECKPOINTS) + unresolved_fit_failures = _unresolved_fit_failure_tasks(profile, rows) + completed: list[str] = [] + selected_group_keys: set[tuple[str, tuple[str, str, int, str]]] = set() + + for spec in profile.sweep_specs: + if spec.key in unresolved_fit_failures: + if allow_sparse: + continue + break + complete = True + task_group_keys: set[ + tuple[str, tuple[str, str, int, str]] + ] = set() + for joint_name in spec.joints: + joint = profile.record_specs[joint_name] + needs_baseline_hold = int(baseline[joint.motor_index]) not in { + 0, + 255, + } + for cycle in range(int(repetitions)): + for direction in directions: + key = (spec.key, joint_name, cycle, direction) + sample_rows = samples.get(key, []) + feedback_bins = { + int(round(float(row.get("feedback_u8", -1)))) + for row in sample_rows + } + if ( + len(feedback_bins) < int(minimum_sweep_bins) + or 0 not in feedback_bins + or 255 not in feedback_bins + ): + complete = False + break + task_group_keys.add(("sample", key)) + if needs_baseline_hold: + if not baselines.get(key): + complete = False + break + task_group_keys.add(("baseline_hold_sample", key)) + if not complete: + break + if not complete: + break + for direction in directions: + key = (spec.key, joint_name, 0, direction) + requested = { + int(row.get("requested_command_u8", -1)) + for row in checkpoints.get(key, []) + } + if requested != expected_checkpoints: + complete = False + break + task_group_keys.add(("steady_command_sample", key)) + if not complete: + break + if not complete: + if allow_sparse: + continue + break + completed.append(spec.key) + selected_group_keys.update(task_group_keys) + + reusable: list[dict[str, Any]] = [] + grouped = { + "sample": samples, + "baseline_hold_sample": baselines, + "steady_command_sample": checkpoints, + } + for kind, key in selected_group_keys: + reusable.extend(grouped[kind][key]) + completed_set = set(completed) + latest_task_attempt = { + (key[0], key[2], key[3]): max( + int(row.get("attempt", 1)) for row in sample_rows + ) + for key, sample_rows in samples.items() + if key[0] in completed_set and sample_rows + } + reusable.extend( + dict(row) + for row in rows + if row.get("kind") == "synchronised_frame" + 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( + next(spec for spec in profile.sweep_specs if spec.key == row["task_name"]) + ), + int(row.get("cycle", -1)), + 0 if row.get("direction") == DIRECTION_DECREASING else 1, + 0 if row.get("kind") == "synchronised_frame" else 1, + str(row.get("joint", "")), + float(row.get("feedback_u8", row.get("requested_command_u8", 0))), + ) + ) + return tuple(completed), tuple(reusable) + + +def _file_sha256(path: str | Path) -> str: + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _steady_records_in_feedback_domain( + records: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Re-index settled poses by feedback for mechanical backlash checks. + + Runtime curves remain indexed by requested command. This companion + representation separates intrinsic hysteresis from the firmware's + direction-dependent command/feedback deadband. Endpoint records have + already passed their motor-specific reach gate, so they are snapped to + the exact endpoints required by the curve fitter. + """ + result: list[dict[str, Any]] = [] + for source in records: + record = dict(source) + requested = int( + record.get("requested_command_u8", record.get("command_u8", -1)) + ) + if requested in {0, 255}: + feedback_index = requested + else: + feedback_index = int( + np.clip( + np.rint(float(record.get("feedback_u8", requested))), + 0, + 255, + ) + ) + record["command_u8"] = feedback_index + result.append(record) + return result + + +def _classify_cross_view_roll_hysteresis( + front_values_deg: Sequence[float], + side_values_deg: Sequence[float], + *, + limit_deg: float, +) -> str: + """Classify a diagnostic without turning it into a formal acceptance.""" + front = np.asarray(front_values_deg, dtype=float) + side = np.asarray(side_values_deg, dtype=float) + if front.size == 0 or side.size == 0: + raise ValueError("cross-view diagnostic requires both camera results") + front_max = float(np.max(front)) + side_max = float(np.max(side)) + if front_max <= limit_deg and side_max <= limit_deg: + return "both_views_within_formal_hysteresis_limit" + if front_max > limit_deg and side_max > limit_deg: + return "both_views_confirm_direction_dependent_pose" + if front_max > limit_deg: + return "front_only_difference_check_roll_tag_bracket_or_front_pnp" + return "side_only_difference_check_side_tag_chain_or_side_pnp" + + +def _fit_failure_is_systematic( + failures: Sequence[Mapping[str, Any]], repetitions: int +) -> bool: + """Identify a repeatable model conflict that more motion cannot repair.""" + if not failures or int(repetitions) <= 1: + return False + if any( + str(item.get("metric", "")) + != "rotation_circle_axis_difference_deg" + for item in failures + ): + return False + cycles = {int(item.get("cycle", -1)) for item in failures} + if cycles != set(range(1, int(repetitions) + 1)): + return False + actual = np.asarray([float(item["actual"]) for item in failures]) + limits = np.asarray([float(item["limit"]) for item in failures]) + if ( + not np.all(np.isfinite(actual)) + or not np.all(np.isfinite(limits)) + or np.any(limits <= 0.0) + ): + return False + # Every independent round is far outside the limit and agrees with the + # others. This is a fixed geometry/PnP model conflict, not random capture + # loss, so two full four-round retries would only reproduce it. + return bool( + np.all(actual >= 1.5 * limits) + and float(np.ptp(actual)) <= max(0.5, 0.2 * float(np.mean(actual))) + ) + + +def _fit_failure_repeats_branch_clusters( + previous: Sequence[Mapping[str, Any]], + current: Sequence[Mapping[str, Any]], +) -> bool: + """Detect the same cross-cycle PnP branch split on two full retries.""" + + def travel_failure( + failures: Sequence[Mapping[str, Any]], + ) -> Mapping[str, Any] | None: + return next( + ( + item + for item in failures + if str(item.get("metric")) == "cycle_travel_range_deg" + and isinstance(item.get("cycle_travel_deg"), Sequence) + ), + None, + ) + + left = travel_failure(previous) + right = travel_failure(current) + if left is None or right is None: + return False + left_values = np.asarray(left["cycle_travel_deg"], dtype=float) + right_values = np.asarray(right["cycle_travel_deg"], dtype=float) + if ( + left_values.shape != right_values.shape + or left_values.size < 3 + or not np.all(np.isfinite(left_values)) + or not np.all(np.isfinite(right_values)) + or np.max(np.abs(left_values - right_values)) > 1.0 + ): + return False + limit = float(right.get("limit", 0.0)) + if limit <= 0.0 or float(np.ptp(right_values)) <= limit: + return False + + previous_by_metric = { + (str(item.get("joint")), str(item.get("metric"))): item + for item in previous + if "actual" in item and "limit" in item + } + stable_model_failures = 0 + for item in current: + key = (str(item.get("joint")), str(item.get("metric"))) + old = previous_by_metric.get(key) + if old is None or key[1] == "cycle_travel_range_deg": + continue + actual = float(item.get("actual", float("nan"))) + old_actual = float(old.get("actual", float("nan"))) + item_limit = float(item.get("limit", 0.0)) + if ( + np.isfinite(actual) + and np.isfinite(old_actual) + and item_limit > 0.0 + and actual > item_limit + and old_actual > item_limit + and abs(actual - old_actual) <= max(0.25, 0.1 * item_limit) + ): + stable_model_failures += 1 + return stable_model_failures >= 1 + + def _robust_pose_payload( poses: Sequence[Mapping[str, Sequence[float]]], ) -> dict[str, list[float]]: @@ -183,6 +943,7 @@ class SweepItem: spec: SweepSpec cycle: int direction: str + precheck: bool = False @property def start_u8(self) -> int: @@ -199,6 +960,123 @@ class ValidationItem: command_u8: int +@dataclass(frozen=True) +class CombinationValidationItem: + name: str + label_zh: str + command_u8: tuple[int, ...] + + +def _combination_validation_items( + baseline: Sequence[int], +) -> tuple[CombinationValidationItem, ...]: + if len(baseline) != 20: + raise ValueError("combination baseline must contain 20 commands") + base = [int(value) for value in baseline] + + def item( + name: str, label: str, changes: Mapping[int, int] + ) -> CombinationValidationItem: + values = list(base) + for index, value in changes.items(): + values[int(index)] = int(value) + return CombinationValidationItem(name, label, tuple(values)) + + return ( + item("all_open", "全开", {}), + item("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), + item("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), + item("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), + item("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), + item("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), + item( + "half_grip", + "四指半握", + {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, + ), + item( + "light_pinch", + "轻捏", + {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, + ), + ) + + +def _combination_joint_angles( + profile: HandCalibrationProfile, + measured_fits: Mapping[str, JointCurveFit], + command_u8: Sequence[int], + motor_directions: Sequence[str], +) -> dict[str, float]: + """Evaluate a combination pose on the branch used to reach it. + + Using ``angle_rad`` here averages the increasing and decreasing branches; + on a serial chain (especially the thumb) those half-hysteresis errors + accumulate and can reject an otherwise correct pose. + """ + if len(command_u8) != 20 or len(motor_directions) != 20: + raise ValueError("combination commands and directions require 20 motors") + angles: dict[str, float] = {} + for name, spec in profile.joint_specs.items(): + target = int(command_u8[spec.motor_index]) + fit = measured_fits[name] + direction = str(motor_directions[spec.motor_index]) + if direction == DIRECTION_DECREASING: + curve = fit.decreasing_rad + elif direction == DIRECTION_INCREASING: + curve = fit.increasing_rad + else: + raise ValueError(f"invalid combination direction: {direction}") + angles[name] = float(curve[target]) + return angles + + +def _combination_motor_directions( + items: Sequence[CombinationValidationItem], + pose_index: int, + baseline_u8: Sequence[int], +) -> tuple[str, ...]: + """Reconstruct the direction in which every motor reached a pose. + + Pose zero establishes the all-open Tag mounts. Each later pose starts + from that baseline. A motor moved by an earlier pose returns along the + opposite branch and stays on that branch until another pose moves it. + """ + if len(baseline_u8) != 20: + raise ValueError("combination baseline must contain 20 motors") + if pose_index < 0 or pose_index >= len(items): + raise ValueError("combination pose index is out of range") + baseline = tuple(int(value) for value in baseline_u8) + directions = [DIRECTION_DECREASING] * 20 + for previous in items[1:pose_index]: + for motor, target in enumerate(previous.command_u8): + if int(target) < baseline[motor]: + directions[motor] = DIRECTION_INCREASING + elif int(target) > baseline[motor]: + directions[motor] = DIRECTION_DECREASING + current = items[pose_index] + for motor, target in enumerate(current.command_u8): + if int(target) < baseline[motor]: + directions[motor] = DIRECTION_DECREASING + elif int(target) > baseline[motor]: + directions[motor] = DIRECTION_INCREASING + return tuple(directions) + + +def _model_link_in_observer_base( + observer_base_common: Sequence[Sequence[float]], + model_base_common: Sequence[Sequence[float]], + model_link_from_base: Sequence[Sequence[float]], +) -> np.ndarray: + """Express a URDF link in the fixed palm Tag's coordinate frame.""" + observer = np.asarray(observer_base_common, dtype=float) + model_base = np.asarray(model_base_common, dtype=float) + link = np.asarray(model_link_from_base, dtype=float) + if any(value.shape != (4, 4) for value in (observer, model_base, link)): + raise ValueError("combination transforms must be 4x4 matrices") + return np.linalg.inv(observer) @ model_base @ link + + @dataclass class ViewRuntime: name: str @@ -222,6 +1100,13 @@ class ViewRuntime: extrinsics_valid: bool = False valid_flags: deque[bool] = field(default_factory=deque) detection_times: deque[float] = field(default_factory=deque) + # Task-scoped capture validity: only frames judged against an active + # task's required roles are counted. The rolling valid_flags window is + # cleared whenever the required-role set changes, so right after a task + # finishes it silently switches to scoring idle frames against the full + # preflight role set — the wrong measurement for a per-task data gate. + task_valid_frames: int = 0 + task_total_frames: int = 0 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 = "" @@ -229,6 +1114,12 @@ class ViewRuntime: pnp_reset_count: int = 0 last_message_at: float = 0.0 last_valid_at: float = 0.0 + fixed_base_observations: deque[ + tuple[SquareTagPose, tuple[float, float], TagQuality] + ] = field(default_factory=deque) + locked_base_pose: SquareTagPose | None = None + locked_base_center_xy_px: tuple[float, float] | None = None + locked_base_quality: TagQuality | None = None def __post_init__(self) -> None: self.role_by_id = { @@ -236,6 +1127,7 @@ class ViewRuntime: } self.valid_flags = deque(maxlen=self.preflight_frames) self.detection_times = deque(maxlen=self.preflight_frames) + self.fixed_base_observations = deque(maxlen=self.preflight_frames) if not self.preflight_roles: self.preflight_roles = tuple(self.view_tags) if any(role not in self.view_tags for role in self.preflight_roles): @@ -282,22 +1174,46 @@ class G20ThreeCameraCalibrationNode(Node): self.latest_state_u8: tuple[float, ...] = () self.last_state_at = 0.0 self.state_history: deque[StateSample] = deque(maxlen=1200) + self.state_receive_times: deque[float] = deque(maxlen=300) self.latest_hand_info: dict[str, Any] = {} self.commanded_speed_profile: tuple[int, ...] = () self.speed_commanded_at = 0.0 self.state = STATE_PREFLIGHT - self.reason = "waiting_for_three_cameras_tags_and_sdk" + self.reason = "waiting_for_devices_and_sdk" self.paused_reason = "" self.started = False + # Startup has two deliberately separate gates. Camera/SDK health is + # enough to let us recover the hand from an interrupted pose; fixed + # palm Tags are checked only after that recovery reaches baseline. + self.startup_baseline_recovered = False + self.abort_original_reason = "" self.baseline_after = "" self.position_hold_since: float | None = None self.motion_stage_started_at = 0.0 + self.return_waypoints: deque[tuple[int, ...]] = deque() + self.preparation_waypoints: deque[tuple[int, ...]] = deque() + self.preparation_command_u8: tuple[int, ...] = tuple( + self.baseline_command + ) self.sweep_items: list[SweepItem] = [] self.sweep_index = 0 self.active_sweep: SweepItem | 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 + # valid backlash observation. Keep a separate buffer populated only + # after the motor has reached 127 and the commanded mid-sweep hold has + # started. + self.sweep_baseline_frames: list[FrameObservation] = [] + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since: float | None = None + self.sweep_checkpoint_commands: deque[int] = deque() + self.sweep_checkpoint_mode = False + self.sweep_checkpoint_target_u8: int | None = None + self.sweep_checkpoint_hold_since: float | None = None + self.sweep_checkpoint_frames: list[FrameObservation] = [] # Keep synchronised endpoint observations acquired while the motor is # held at the sweep start. If these frames are discarded and the # target is commanded immediately, a fast motor can leave the endpoint @@ -305,7 +1221,16 @@ class G20ThreeCameraCalibrationNode(Node): self.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] = {} self.sweep_endpoint_since: float | None = None + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view: dict[str, int] = {} + self.sweep_detection_valid_by_view: dict[str, int] = {} + self.precheck_speed_metrics: dict[ + str, dict[str, dict[str, float | int]] + ] = {} + self.formal_speed_scales: dict[str, float] = {} self.motion_progress_reference_error_u8 = float("inf") self.motion_last_progress_at = 0.0 self.motion_stall_details: dict[str, Any] = {} @@ -314,14 +1239,24 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_items: list[SweepItem] = [] self.active_sweep_is_fit_retry = False self.fit_failure: dict[str, Any] = {} - self.sweep_attempts: dict[int, int] = { - spec.motor_index: 1 for spec in self.profile.sweep_specs + self.fit_failure_history_by_task: dict[ + str, list[list[dict[str, Any]]] + ] = {} + self.cross_view_roll_diagnostic_result: dict[str, Any] = {} + self.sweep_attempts: dict[str | int, int] = { + _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs } - self.sweep_retry_counts: dict[tuple[int, int, str], int] = {} + self.sweep_retry_counts: dict[tuple[str | int, int, str], int] = {} self.motion_retry_counts: dict[str, int] = {} - self.validation_retry_counts: dict[tuple[int, int], int] = {} + self.validation_retry_counts: dict[tuple[str | int, int], int] = {} self.records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.measured_joints + name: [] for name in self.profile.record_joints + } + self.baseline_records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in self.profile.record_joints + } + self.command_records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in self.profile.record_joints } self.measured_fits: dict[str, JointCurveFit] = {} @@ -333,9 +1268,22 @@ class G20ThreeCameraCalibrationNode(Node): self.validation_items: list[ValidationItem] = [] self.validation_index = 0 self.active_validation: ValidationItem | None = None + self.active_validation_direction: str | None = None self.validation_frames_buffer: list[FrameObservation] = [] self.validation_errors_rad: list[float] = [] self.validation_stage_started_at = 0.0 + self.combination_validation_items: list[CombinationValidationItem] = [] + self.combination_validation_index = 0 + self.active_combination_validation: CombinationValidationItem | None = None + self.combination_validation_frames_buffer: dict[ + str, list[FrameObservation] + ] = {view: [] for view in ("front", "side", "top")} + self.combination_validation_completed = False + self.combination_tag_mounts: dict[str, np.ndarray] = {} + self.combination_tag_observation_counts: dict[str, int] = {} + self.combination_tag_validation_counts: dict[str, int] = {} + self.combination_position_errors_m: list[float] = [] + self.combination_orientation_errors_rad: list[float] = [] self.completed_payload: dict[str, Any] | None = None self.command_publisher = self.create_publisher( @@ -385,10 +1333,13 @@ class G20ThreeCameraCalibrationNode(Node): def _declare_parameters(self) -> None: self.declare_parameter("hand_type", "left") + self.declare_parameter("tag_layout", "legacy_11") self.declare_parameter("serial_number", "UNSET") self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("resume_raw_samples_path", "") self.declare_parameter("camera_extrinsics_file", "") self.declare_parameter("source_urdf_path", "") + self.declare_parameter("source_urdf_expected_sha256", "") self.declare_parameter("corrected_urdf_output_dir", "") self.declare_parameter("commands_enabled", True) self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") @@ -414,11 +1365,18 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("normal_calibration_speed", 15) self.declare_parameter("index_roll_calibration_speed", 5) self.declare_parameter("index_flex_calibration_speed", 10) + self.declare_parameter("adaptive_formal_speed_enabled", True) + self.declare_parameter("adaptive_formal_speed_max_scale", 1.5) + self.declare_parameter("adaptive_formal_speed_minimum_bins", 64) + self.declare_parameter("adaptive_formal_speed_maximum_bin_gap", 8) self.declare_parameter("speed_setting_settle_seconds", 0.25) + self.declare_parameter("parallel_pose_transitions", True) self.declare_parameter("repetitions", 3) + self.declare_parameter("g20_right_19_repetitions", 4) self.declare_parameter("preflight_frames", 60) self.declare_parameter("minimum_detection_rate", 0.95) self.declare_parameter("minimum_detection_hz", 15.0) + self.declare_parameter("minimum_feedback_hz", 25.0) self.declare_parameter("maximum_hamming", 0) self.declare_parameter("minimum_decision_margin", 30.0) self.declare_parameter("minimum_edge_pixels", 30.0) @@ -431,6 +1389,11 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pnp_group_initialization_frames", 8) self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) + self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03) + self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0) + self.declare_parameter( + "thumb_ip_pnp_maximum_coupling_residual_deg", 7.5 + ) self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) self.declare_parameter("maximum_state_image_skew_ms", 50.0) self.declare_parameter("axis_maximum_plane_rms_m", 0.003) @@ -446,9 +1409,18 @@ class G20ThreeCameraCalibrationNode(Node): ) self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) + self.declare_parameter( + "zero_maximum_observability_condition_number", 1.0e10 + ) self.declare_parameter("zero_maximum_offset_deg", 20.0) self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter( + "steady_checkpoint_command_feedback_tolerance_u8", 8.0 + ) + self.declare_parameter( + "steady_checkpoint_maximum_feedback_range_u8", 2.0 + ) self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) self.declare_parameter( "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 @@ -456,9 +1428,12 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pinky_pip_zero_endpoint_tolerance_u8", 5.0) self.declare_parameter("endpoint_hold_seconds", 0.5) self.declare_parameter("baseline_hold_seconds", 0.5) + self.declare_parameter("minimum_baseline_hold_frames", 10) + self.declare_parameter("task_precheck_hold_seconds", 2.0) self.declare_parameter("position_timeout_seconds", 30.0) self.declare_parameter("sweep_timeout_seconds", 90.0) - self.declare_parameter("motor_stall_timeout_seconds", 8.0) + self.declare_parameter("motor_stall_timeout_seconds", 5.0) + self.declare_parameter("motor_stall_startup_grace_seconds", 1.0) self.declare_parameter("motor_stall_minimum_progress_u8", 1.0) self.declare_parameter("invalid_timeout_seconds", 3.0) self.declare_parameter("minimum_sweep_frames", 40) @@ -468,6 +1443,7 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("automatic_sweep_retry_limit", 3) self.declare_parameter("automatic_fit_retry_limit", 2) self.declare_parameter("automatic_motion_retry_limit", 2) + self.declare_parameter("cross_view_roll_diagnostic_finger", "") self.declare_parameter("provisional_warning_ratio", 1.25) self.declare_parameter("retry_minimum_speed", 3) self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5]) @@ -489,37 +1465,80 @@ class G20ThreeCameraCalibrationNode(Node): ) self.declare_parameter("maximum_monotonic_correction_deg", 2.0) self.declare_parameter("maximum_hysteresis_deg", 5.0) + self.declare_parameter("baseline_maximum_hysteresis_deg", 0.5) + self.declare_parameter( + "directional_zero_maximum_branch_gap_deg", 1.5 + ) + self.declare_parameter( + "directional_zero_maximum_branch_gap_range_deg", 0.3 + ) + self.declare_parameter( + "cross_view_roll_maximum_branch_gap_difference_deg", 0.3 + ) + 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) self.declare_parameter("validation_enabled", False) + self.declare_parameter("combination_validation_enabled", True) + self.declare_parameter("combination_validation_frames", 10) + self.declare_parameter("combination_maximum_position_p95_m", 0.003) + self.declare_parameter("combination_maximum_orientation_p95_deg", 2.0) self.declare_parameter("validation_command_count", 3) self.declare_parameter("validation_frames", 10) self.declare_parameter("validation_seed", 20260804) self.declare_parameter("validation_timeout_seconds", 20.0) self.declare_parameter("maximum_validation_mae_deg", 1.0) self.declare_parameter("maximum_validation_p95_deg", 2.0) + self.declare_parameter("maximum_validation_error_deg", 1.5) + self.declare_parameter("zero_maximum_confidence_half_width_deg", 0.5) def _load_parameters(self) -> None: def value(name: str) -> Any: return self.get_parameter(name).value 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.hand_type, self.tag_layout ) self.zero_profile: ZeroCalibrationProfile = ( - get_zero_calibration_profile(self.hand_type) + get_zero_calibration_profile(self.hand_type, self.tag_layout) ) self.serial_number = str(value("serial_number")) if self.serial_number == "UNSET": raise ValueError("serial_number is required") self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + resume_value = str(value("resume_raw_samples_path")).strip() + self.resume_raw_samples_path = ( + None + if not resume_value + else Path(resume_value).expanduser().resolve() + ) + self.resumed_task_keys: tuple[str, ...] = () + self.resume_source_session = "" self.camera_extrinsics_file = Path( str(value("camera_extrinsics_file")) ).expanduser().resolve() self.source_urdf_path = Path( str(value("source_urdf_path")) ).expanduser().resolve() + self.source_urdf_expected_sha256 = str( + value("source_urdf_expected_sha256") + ).strip().lower() output_value = str(value("corrected_urdf_output_dir")) self.corrected_urdf_output_dir = ( Path(output_value).expanduser().resolve() @@ -570,13 +1589,28 @@ class G20ThreeCameraCalibrationNode(Node): self.index_flex_calibration_speed = int( value("index_flex_calibration_speed") ) + self.adaptive_formal_speed_enabled = bool( + value("adaptive_formal_speed_enabled") + ) + self.adaptive_formal_speed_max_scale = float( + value("adaptive_formal_speed_max_scale") + ) + self.adaptive_formal_speed_minimum_bins = int( + value("adaptive_formal_speed_minimum_bins") + ) + self.adaptive_formal_speed_maximum_bin_gap = int( + value("adaptive_formal_speed_maximum_bin_gap") + ) self.speed_setting_settle_seconds = float( value("speed_setting_settle_seconds") ) self.repetitions = int(value("repetitions")) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + self.repetitions = int(value("g20_right_19_repetitions")) self.preflight_frames = int(value("preflight_frames")) self.minimum_detection_rate = float(value("minimum_detection_rate")) self.minimum_detection_hz = float(value("minimum_detection_hz")) + self.minimum_feedback_hz = float(value("minimum_feedback_hz")) self.maximum_hamming = int(value("maximum_hamming")) self.minimum_decision_margin = float(value("minimum_decision_margin")) self.minimum_edge_pixels = float(value("minimum_edge_pixels")) @@ -603,6 +1637,15 @@ class G20ThreeCameraCalibrationNode(Node): self.pnp_group_maximum_normal_alignment_rad = math.radians( float(value("pnp_group_maximum_normal_alignment_deg")) ) + self.thumb_ip_pnp_coupling_multiplier = float( + value("thumb_ip_pnp_coupling_multiplier") + ) + self.thumb_ip_pnp_coupling_scale_rad = math.radians( + float(value("thumb_ip_pnp_coupling_scale_deg")) + ) + self.thumb_ip_pnp_maximum_coupling_residual_rad = math.radians( + float(value("thumb_ip_pnp_maximum_coupling_residual_deg")) + ) self.top_pnp_invalid_reset_seconds = float( value("top_pnp_invalid_reset_seconds") ) @@ -636,6 +1679,9 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cone_mismatch_rad = math.radians( float(value("zero_maximum_axis_cone_mismatch_deg")) ) + self.zero_maximum_observability_condition_number = float( + value("zero_maximum_observability_condition_number") + ) self.zero_maximum_offset_rad = math.radians( float(value("zero_maximum_offset_deg")) ) @@ -644,6 +1690,12 @@ class G20ThreeCameraCalibrationNode(Node): ) self.zero_joint_maximum_offsets_rad: dict[str, float] = {} self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.steady_checkpoint_command_feedback_tolerance_u8 = float( + value("steady_checkpoint_command_feedback_tolerance_u8") + ) + self.steady_checkpoint_maximum_feedback_range_u8 = float( + value("steady_checkpoint_maximum_feedback_range_u8") + ) self.thumb_yaw_zero_endpoint_tolerance_u8 = float( value("thumb_yaw_zero_endpoint_tolerance_u8") ) @@ -655,11 +1707,23 @@ class G20ThreeCameraCalibrationNode(Node): ) self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) self.baseline_hold_seconds = float(value("baseline_hold_seconds")) + self.minimum_baseline_hold_frames = int( + value("minimum_baseline_hold_frames") + ) + self.task_precheck_hold_seconds = float( + value("task_precheck_hold_seconds") + ) self.position_timeout_seconds = float(value("position_timeout_seconds")) + self.parallel_pose_transitions = bool( + value("parallel_pose_transitions") + ) self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) self.motor_stall_timeout_seconds = float( value("motor_stall_timeout_seconds") ) + self.motor_stall_startup_grace_seconds = float( + value("motor_stall_startup_grace_seconds") + ) self.motor_stall_minimum_progress_u8 = float( value("motor_stall_minimum_progress_u8") ) @@ -674,6 +1738,9 @@ class G20ThreeCameraCalibrationNode(Node): self.automatic_fit_retry_limit = int( value("automatic_fit_retry_limit") ) + self.cross_view_roll_diagnostic_finger = str( + value("cross_view_roll_diagnostic_finger") + ).strip().lower() self.automatic_motion_retry_limit = int( value("automatic_motion_retry_limit") ) @@ -720,13 +1787,49 @@ class G20ThreeCameraCalibrationNode(Node): self.maximum_hysteresis_rad = math.radians( float(value("maximum_hysteresis_deg")) ) + self.baseline_maximum_hysteresis_rad = math.radians( + float(value("baseline_maximum_hysteresis_deg")) + ) + self.directional_zero_maximum_branch_gap_rad = math.radians( + float(value("directional_zero_maximum_branch_gap_deg")) + ) + self.directional_zero_maximum_branch_gap_range_rad = math.radians( + float(value("directional_zero_maximum_branch_gap_range_deg")) + ) + 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_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")) ) self.passive_maximum_hysteresis_rad = math.radians( float(value("passive_maximum_hysteresis_deg")) ) + self.command_maximum_direction_gap_rad = math.radians( + float(value("command_maximum_direction_gap_deg")) + ) self.validation_enabled = bool(value("validation_enabled")) + self.combination_validation_enabled = bool( + value("combination_validation_enabled") + ) + self.combination_validation_frames = int( + value("combination_validation_frames") + ) + self.combination_maximum_position_p95_m = float( + value("combination_maximum_position_p95_m") + ) + self.combination_maximum_orientation_p95_rad = math.radians( + float(value("combination_maximum_orientation_p95_deg")) + ) self.validation_command_count = int(value("validation_command_count")) self.validation_frames = int(value("validation_frames")) self.validation_seed = int(value("validation_seed")) @@ -739,6 +1842,12 @@ class G20ThreeCameraCalibrationNode(Node): self.maximum_validation_p95_rad = math.radians( float(value("maximum_validation_p95_deg")) ) + self.maximum_validation_error_rad = math.radians( + float(value("maximum_validation_error_deg")) + ) + 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 any(value < 0 or value > 255 for value in self.baseline_command): @@ -753,6 +1862,31 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError( f"source_urdf_path does not exist: {self.source_urdf_path}" ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + if re.fullmatch( + r"[0-9a-f]{64}", self.source_urdf_expected_sha256 + ) is None: + raise ValueError( + "g20_right_15 requires source_urdf_expected_sha256 from " + "the CAD/hardware owner; unconfirmed source CAD cannot " + "produce a formal corrected URDF" + ) + actual_source_hash = _file_sha256(self.source_urdf_path) + if actual_source_hash != self.source_urdf_expected_sha256: + raise ValueError( + "source_urdf_path SHA-256 does not match the confirmed " + "G20 CAD hash" + ) + source_model = UrdfKinematicModel(self.source_urdf_path) + thumb_ip_model = source_model.joints.get("thumb_ip") + if ( + thumb_ip_model is None + or thumb_ip_model.mimic_joint != "thumb_mcp" + or abs(thumb_ip_model.mimic_offset) > 1.0e-9 + ): + raise ValueError( + "source URDF thumb_ip mimic structure is invalid" + ) source_name = self.source_urdf_path.name.lower() opposite = "right" if self.hand_type == "left" else "left" if f"g20_{opposite}" in source_name: @@ -775,10 +1909,44 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("index_roll_calibration_speed must be in [0, 255]") if not 0 <= self.index_flex_calibration_speed <= 255: raise ValueError("index_flex_calibration_speed must be in [0, 255]") + if not 1.0 <= self.adaptive_formal_speed_max_scale <= 2.0: + raise ValueError( + "adaptive_formal_speed_max_scale must be in [1, 2]" + ) + if ( + self.adaptive_formal_speed_minimum_bins + < self.minimum_sweep_bins + ): + raise ValueError( + "adaptive_formal_speed_minimum_bins cannot be below the " + "formal minimum_sweep_bins" + ) + if not ( + 1 + <= self.adaptive_formal_speed_maximum_bin_gap + <= self.maximum_bin_gap + ): + raise ValueError( + "adaptive_formal_speed_maximum_bin_gap must be in " + "[1, maximum_bin_gap]" + ) if self.speed_setting_settle_seconds < 0.0: 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 ( + self.endpoint_tolerance_u8 + <= self.steady_checkpoint_command_feedback_tolerance_u8 + <= 16.0 + ): + raise ValueError( + "steady_checkpoint_command_feedback_tolerance_u8 must be " + "between endpoint_tolerance_u8 and 16" + ) + if not 0.0 < self.steady_checkpoint_maximum_feedback_range_u8 <= 4.0: + raise ValueError( + "steady_checkpoint_maximum_feedback_range_u8 must be in (0, 4]" + ) if not ( self.endpoint_tolerance_u8 <= self.thumb_yaw_zero_endpoint_tolerance_u8 @@ -814,6 +1982,8 @@ class G20ThreeCameraCalibrationNode(Node): ) if self.motor_stall_timeout_seconds <= 0.0: raise ValueError("motor_stall_timeout_seconds must be positive") + if not 0.0 <= self.motor_stall_startup_grace_seconds <= 5.0: + raise ValueError("motor_stall_startup_grace_seconds must be in [0, 5]") if not 0.0 < self.motor_stall_minimum_progress_u8 <= 10.0: raise ValueError( "motor_stall_minimum_progress_u8 must be in (0, 10]" @@ -826,6 +1996,9 @@ class G20ThreeCameraCalibrationNode(Node): self.image_trajectory_minimum_radius_px, self.pnp_group_normal_alignment_scale_rad, self.pnp_group_maximum_normal_alignment_rad, + self.thumb_ip_pnp_coupling_multiplier, + self.thumb_ip_pnp_coupling_scale_rad, + self.thumb_ip_pnp_maximum_coupling_residual_rad, self.axis_maximum_plane_rms_m, self.passive_axis_maximum_plane_rms_m, self.axis_maximum_pose_line_rms_m, @@ -833,12 +2006,20 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cone_mismatch_rad, self.zero_maximum_offset_rad, self.zero_finger_maximum_offset_rad, + self.maximum_validation_error_rad, + self.zero_maximum_confidence_half_width_rad, + self.task_precheck_hold_seconds, self.active_maximum_rotation_orthogonal_rms_rad, self.passive_maximum_rotation_orthogonal_rms_rad, self.trajectory_maximum_cycle_travel_difference_rad, self.passive_maximum_cycle_travel_difference_rad, + self.baseline_maximum_hysteresis_rad, + 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.passive_maximum_monotonic_correction_rad, self.passive_maximum_hysteresis_rad, + self.command_maximum_direction_gap_rad, ) ): raise ValueError("trajectory quality thresholds must be positive") @@ -847,16 +2028,47 @@ class G20ThreeCameraCalibrationNode(Node): "zero_finger_maximum_offset_deg cannot exceed " "zero_maximum_offset_deg" ) - if self.repetitions != 3: - raise ValueError("three-camera calibration requires exactly 3 repetitions") + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.repetitions < 4 + ): + raise ValueError( + "g20_right_15 requires at least 4 repetitions: at least 3 " + "training cycles and one isolated holdout" + ) + if ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + and self.repetitions != 3 + ): + raise ValueError( + "legacy three-camera calibration requires exactly 3 repetitions" + ) if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: raise ValueError("preflight_frames or minimum_sweep_bins is too small") + if self.minimum_feedback_hz <= 0.0: + raise ValueError("minimum_feedback_hz must be positive") + if self.minimum_baseline_hold_frames < 3: + raise ValueError("minimum_baseline_hold_frames must be at least 3") if not 0 <= self.automatic_sweep_retry_limit <= 5: raise ValueError("automatic_sweep_retry_limit must be in [0, 5]") if not 0 <= self.automatic_fit_retry_limit <= 2: raise ValueError("automatic_fit_retry_limit must be in [0, 2]") if not 0 <= self.automatic_motion_retry_limit <= 5: raise ValueError("automatic_motion_retry_limit must be in [0, 5]") + if self.cross_view_roll_diagnostic_finger not in { + "", "pinky", "ring", "middle", "index" + }: + raise ValueError( + "cross_view_roll_diagnostic_finger must be empty or one of " + "pinky/ring/middle/index" + ) + if ( + self.cross_view_roll_diagnostic_finger + and self.profile.layout_id != G20_RIGHT_19_LAYOUT + ): + raise ValueError( + "cross-view roll diagnostic requires g20_right_15" + ) if not 1.0 <= self.provisional_warning_ratio <= 2.0: raise ValueError("provisional_warning_ratio must be in [1, 2]") if not 1 <= self.retry_minimum_speed <= 255: @@ -878,6 +2090,13 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("validation_command_count must be in [1, 10]") if self.validation_frames < 3: raise ValueError("validation_frames must be at least 3") + if self.combination_validation_frames < 3: + raise ValueError("combination_validation_frames must be at least 3") + if ( + self.combination_maximum_position_p95_m <= 0.0 + or self.combination_maximum_orientation_p95_rad <= 0.0 + ): + raise ValueError("combination pose validation thresholds must be positive") def _make_group_pose_tracker( self, name: str, roles: Sequence[str] @@ -886,7 +2105,14 @@ class G20ThreeCameraCalibrationNode(Node): role_set = set(role_tuple) adjacent_pairs = tuple( pair - for pair in _view_pairs(name, self.profile.reference_finger) + for pair in dict.fromkeys( + (spec.parent_role, spec.child_role) + for spec in self.profile.record_specs.values() + if spec.measured + and spec.view == name + and spec.parent_role is not None + and spec.child_role is not None + ) if pair[0] in role_set and pair[1] in role_set ) # The side-view palm/reference-finger Tags are mounted on parallel @@ -895,8 +2121,20 @@ class G20ThreeCameraCalibrationNode(Node): # static IPPE mirror branches; their face-normal consistency can. Do # not apply this prior to the articulated front thumb chain. normal_alignment_pairs = ( - adjacent_pairs if name == "side" else () + adjacent_pairs if name == "side" and len(role_tuple) <= 4 else () ) + 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): + thumb_mcp_ip_coupling = ( + ( + "thumb_cmc", + "thumb_mcp", + "thumb_mcp", + "thumb_ip", + self.thumb_ip_pnp_coupling_multiplier, + ), + ) return SquareTagGroupPoseTracker( roles=role_tuple, adjacent_pairs=adjacent_pairs, @@ -922,7 +2160,19 @@ class G20ThreeCameraCalibrationNode(Node): ), maximum_normal_alignment_rad=( self.pnp_group_maximum_normal_alignment_rad - if name == "side" + if normal_alignment_pairs + else None + ), + # The source-URDF thumb_ip mimic ratio is used only to disambiguate + # the two planar-IPPE candidates while motor 15 drives MCP and IP + # together. If no candidate matches this weak prior, selection + # falls back to visual continuity instead of dropping the frame. + # Accepted Tag poses still fit both visual curves independently. + coupled_rotation_pairs=thumb_mcp_ip_coupling, + coupled_rotation_scale_rad=self.thumb_ip_pnp_coupling_scale_rad, + maximum_coupled_rotation_residual_rad=( + self.thumb_ip_pnp_maximum_coupling_residual_rad + if thumb_mcp_ip_coupling else None ), ) @@ -954,32 +2204,172 @@ class G20ThreeCameraCalibrationNode(Node): def _required_roles_for_view(self, view: str) -> tuple[str, ...]: """Return only the roles required by the active task in this view.""" runtime = self.views[view] + if getattr(self, "active_combination_validation", None) is not None: + # Combination validation consumes every reliable visible target, + # but only the fixed palm reference is a blocking requirement. + # Four adjacent fingers cannot expose all of their Tags to one + # side camera at the same instant. + return tuple(runtime.preflight_roles) active_spec: SweepSpec | None = None - if self.active_sweep is not None and self.active_sweep.spec.view == view: + if ( + self.active_sweep is not None + and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + ): active_spec = self.active_sweep.spec elif ( self.active_validation is not None - and self.active_validation.spec.view == view + and view in _sweep_views( + self.profile, self.active_validation.spec + ) ): active_spec = self.active_validation.spec - elif self.retry_sweep_spec is not None and self.retry_sweep_spec.view == view: + elif ( + self.retry_sweep_spec is not None + and view in _sweep_views(_node_profile(self), self.retry_sweep_spec) + ): active_spec = self.retry_sweep_spec if active_spec is None: return tuple(runtime.preflight_roles) required: set[str] = set() - for joint_name in active_spec.joints: - joint = self.profile.joint_specs[joint_name] + for joint_name in _sweep_joints_for_view( + self.profile, active_spec, view + ): + joint = self.profile.record_specs[joint_name] if joint.parent_role is not None: required.add(joint.parent_role) if joint.child_role is not None: required.add(joint.child_role) - if view == "side": - # Keep the rigid palm Tag in every side-view task. Otherwise the - # PIP/DIP task initializes Tags 5/6/7 as an unanchored group and a - # stable all-mirror IPPE solution can survive temporal checks. - required.add("side_base") + # Every task keeps its view's fixed palm Tag. Besides suppressing an + # unanchored IPPE mirror branch, this makes the thumb MCP/IP task obey + # its explicit front 0/1/2/3 visibility contract. + base_role = { + "front": "front_base", + "side": "side_base", + "top": "top_base", + }[view] + required.add(base_role) return tuple(role for role in runtime.roles if role in required) + def _locked_base_role_for_active_capture(self, view: str) -> str | None: + runtime = self.views[view] + if ( + getattr(runtime, "locked_base_pose", None) is None + or getattr(runtime, "locked_base_center_xy_px", None) is None + or getattr(runtime, "locked_base_quality", None) is None + ): + return None + if getattr(self, "active_combination_validation", None) is not None: + return _fixed_base_role(view) + spec: SweepSpec | None = None + if self.active_sweep is not None: + spec = self.active_sweep.spec + elif self.active_validation is not None: + spec = self.active_validation.spec + elif self.retry_sweep_spec is not None: + spec = self.retry_sweep_spec + if spec is None or not _sweep_uses_locked_base_reference( + _node_profile(self), spec, view + ): + return None + return _fixed_base_role(view) + + def _live_required_roles_for_view( + self, view: str, required_roles: Sequence[str] + ) -> tuple[str, ...]: + locked = self._locked_base_role_for_active_capture(view) + return tuple(role for role in required_roles if role != locked) + + def _lock_fixed_base_references(self) -> bool: + """Freeze robust palm references captured at the confirmed baseline.""" + minimum = min(30, int(self.preflight_frames)) + pending = False + for view, runtime in self.views.items(): + if runtime.locked_base_pose is not None: + continue + observations = list(runtime.fixed_base_observations) + if len(observations) < minimum: + pending = True + continue + poses = [item[0] for item in observations] + centres = np.asarray([item[1] for item in observations], dtype=float) + qualities = [item[2] for item in observations] + translation = np.median( + np.asarray( + [pose.translation_xyz_m for pose in poses], dtype=float + ), + axis=0, + ) + quaternion = robust_rotation_summary( + [pose.quaternion_xyzw for pose in poses] + )[0] + reprojection = float( + np.percentile( + [pose.reprojection_error_px for pose in poses], 95.0 + ) + ) + runtime.locked_base_pose = SquareTagPose( + tuple(float(value) for value in quaternion), + tuple(float(value) for value in translation), + reprojection, + ) + runtime.locked_base_center_xy_px = tuple( + float(value) for value in np.median(centres, axis=0) + ) + runtime.locked_base_quality = TagQuality( + hamming=max(item.hamming for item in qualities), + decision_margin=min(item.decision_margin for item in qualities), + edge_pixels=min(item.edge_pixels for item in qualities), + reprojection_error_px=reprojection, + ) + translation_rms_m = float( + np.sqrt( + np.mean( + np.sum( + ( + np.asarray( + [pose.translation_xyz_m for pose in poses], + dtype=float, + ) + - translation + ) + ** 2, + axis=1, + ) + ) + ) + ) + rotation_p95_deg = math.degrees( + float( + np.percentile( + [ + ( + Rotation.from_quat(quaternion).inv() + * Rotation.from_quat(pose.quaternion_xyzw) + ).magnitude() + for pose in poses + ], + 95.0, + ) + ) + ) + append_jsonl( + self.raw_path, + { + "kind": "fixed_base_reference_locked", + "view": view, + "role": _fixed_base_role(view), + "tag_id": runtime.view_tags[_fixed_base_role(view)], + "sample_count": len(observations), + "translation_rms_m": round(translation_rms_m, 9), + "rotation_p95_deg": round(rotation_p95_deg, 6), + "reprojection_p95_px": round(reprojection, 6), + }, + ) + return not pending and all( + runtime.locked_base_pose is not None + for runtime in self.views.values() + ) + def _group_tracker_for_roles( self, runtime: ViewRuntime, roles: Sequence[str] ) -> SquareTagGroupPoseTracker: @@ -991,10 +2381,14 @@ class G20ThreeCameraCalibrationNode(Node): return group @staticmethod - def _reset_view_trackers(runtime: ViewRuntime) -> None: + def _reset_view_trackers( + runtime: ViewRuntime, *, preserve_task_reference: bool = False + ) -> None: runtime.tracker.reset() for group in runtime.group_trackers.values(): - group.reset() + group.reset( + preserve_task_reference=preserve_task_reference + ) def _camera_info_callback(self, view: str, message: CameraInfo) -> None: runtime = self.views[view] @@ -1059,6 +2453,7 @@ class G20ThreeCameraCalibrationNode(Node): stamp = int(self.get_clock().now().nanoseconds) self.latest_state_u8 = state self.last_state_at = time.monotonic() + self.state_receive_times.append(self.last_state_at) if not self.state_history or stamp > self.state_history[-1].stamp_ns: self.state_history.append(StateSample(stamp, state)) @@ -1088,12 +2483,22 @@ class G20ThreeCameraCalibrationNode(Node): now = time.monotonic() stamp = _stamp_ns(message.header.stamp) required_roles = self._required_roles_for_view(view) + locked_base_role = self._locked_base_role_for_active_capture(view) + live_required_roles = self._live_required_roles_for_view( + view, required_roles + ) + combination_capture = bool( + self.active_combination_validation is not None + and self.state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE} + ) if required_roles != runtime.current_required_roles: runtime.current_required_roles = required_roles runtime.valid_flags.clear() runtime.detection_times.clear() runtime.latest_pnp_rejections.clear() runtime.latest_group_pnp_reason = "" + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 self._reset_view_trackers(runtime) qualities: dict[str, TagQuality] = {} corners_by_role: dict[str, np.ndarray] = {} @@ -1117,17 +2522,88 @@ class G20ThreeCameraCalibrationNode(Node): corners_by_role[role] = corners centres_by_role[role] = np.mean(corners, axis=0) - detection_good = all(role in qualities for role in required_roles) and all( - self._quality_valid(qualities[role], include_pnp=False) - for role in required_roles + pose_roles = ( + tuple( + role + for role in runtime.roles + if role in qualities + and role != locked_base_role + and self._quality_valid(qualities[role], include_pnp=False) + ) + if combination_capture + else live_required_roles ) + observable_combination_joints = ( + _combination_observable_joints( + self.profile, + view, + ( + *pose_roles, + *((locked_base_role,) if locked_base_role else ()), + ), + ) + if combination_capture + else () + ) + detection_good = bool( + all(role in qualities for role in live_required_roles) + and all( + self._quality_valid(qualities[role], include_pnp=False) + for role in live_required_roles + ) + and ( + not combination_capture + or bool(observable_combination_joints) + ) + ) + tracking_command_u8: int | None = None + tracking_direction: str | None = None + 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 self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + ): + matched_tracking = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched_tracking is not None: + matched_tracking_state = matched_tracking[0] + if ( + self.state == STATE_SWEEP + or self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.start_u8, + matched_tracking_state, + ) + ): + # During PREPARE, learn only the final held start pose. + # In particular a roll task approaches 255 from its 127 + # baseline; caching that preparation motion as the formal + # decreasing trajectory would contaminate the same-command + # return reference before the real scan overwrites it. + tracking_command_u8 = int( + np.clip( + np.rint( + matched_tracking_state[ + self.active_sweep.spec.motor_index + ] + ), + 0, + 255, + ) + ) + tracking_direction = self.active_sweep.direction selected: dict[str, SquareTagPose] | None = None pnp_rejections: dict[str, str] = {} group_pnp_reason = "" if detection_good and runtime.camera_matrix is not None: candidates: dict[str, tuple[SquareTagPose, ...]] = {} - for role in required_roles: - _, rejection = runtime.tracker.estimate( + individual_selected: dict[str, SquareTagPose] = {} + for role in pose_roles: + pose, rejection = runtime.tracker.estimate( role, corners_by_role[role], tag_size_m=runtime.tag_size_m, @@ -1136,30 +2612,94 @@ class G20ThreeCameraCalibrationNode(Node): ) 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, () ) - group_tracker = self._group_tracker_for_roles( - runtime, required_roles - ) - selected, group_pnp_reason = group_tracker.select( - candidates, stamp_ns=stamp - ) - if selected is not None: - for role, pose in selected.items(): - qualities[role] = TagQuality( - hamming=qualities[role].hamming, - decision_margin=qualities[role].decision_margin, - edge_pixels=qualities[role].edge_pixels, - reprojection_error_px=pose.reprojection_error_px, + if combination_capture: + # Each visible target is tracked independently. This avoids + # an exponential all-Tag IPPE search and, more importantly, + # lets an occluded neighbouring finger remain optional. + selected = individual_selected + else: + if locked_base_role is not None: + assert runtime.locked_base_pose is not None + candidates[locked_base_role] = ( + runtime.locked_base_pose, ) + group_tracker = self._group_tracker_for_roles( + runtime, required_roles + ) + selected, group_pnp_reason = group_tracker.select( + candidates, + stamp_ns=stamp, + trajectory_command_u8=tracking_command_u8, + trajectory_direction=tracking_direction, + ) + if selected is not None: + qualities = _selected_pose_qualities( + selected, + qualities, + locked_base_role=locked_base_role, + locked_base_quality=runtime.locked_base_quality, + ) + # Preserve exactly which Tags were present in this camera message. + # The cached palm reference participates in geometry and quality + # checks below, but must never be reported as a live detection. + live_qualities = { + role: quality + for role, quality in qualities.items() + if role in corners_by_role + } + if selected is not None and locked_base_role is not None: + assert runtime.locked_base_pose is not None + assert runtime.locked_base_center_xy_px is not None + assert runtime.locked_base_quality is not None + selected = dict(selected) + selected[locked_base_role] = runtime.locked_base_pose + centres_by_role[locked_base_role] = np.asarray( + runtime.locked_base_center_xy_px, dtype=float + ) + qualities[locked_base_role] = runtime.locked_base_quality valid = bool( selected is not None + and all(role in selected for role in required_roles) and all( self._quality_valid(qualities[role], include_pnp=True) - for role in required_roles + for role in selected + ) + and ( + not combination_capture + or bool( + _combination_observable_joints( + self.profile, view, tuple(selected) + ) + ) ) ) + if ( + self.state == STATE_SWEEP + and self.active_sweep is not None + and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + ): + self.sweep_detection_total_frames += 1 + total_by_view = getattr( + self, "sweep_detection_total_by_view", None + ) + if total_by_view is None: + self.sweep_detection_total_by_view = {} + total_by_view = self.sweep_detection_total_by_view + total_by_view[view] = total_by_view.get(view, 0) + 1 + if valid: + self.sweep_detection_valid_frames += 1 + valid_by_view = getattr( + self, "sweep_detection_valid_by_view", None + ) + if valid_by_view is None: + self.sweep_detection_valid_by_view = {} + valid_by_view = self.sweep_detection_valid_by_view + 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 view == "top": @@ -1182,10 +2722,39 @@ class G20ThreeCameraCalibrationNode(Node): f"group_reason={group_pnp_reason or 'none'}, " f"tag_rejections={pnp_rejections})" ) - runtime.latest_tag_quality = qualities + if ( + valid + and selected is not None + and self.state == STATE_PREFLIGHT + and self.started + and self.startup_baseline_recovered + ): + base_role = _fixed_base_role(view) + if ( + base_role in selected + and base_role in centres_by_role + and base_role in live_qualities + ): + runtime.fixed_base_observations.append( + ( + selected[base_role], + tuple( + float(value) + for value in centres_by_role[base_role] + ), + live_qualities[base_role], + ) + ) + runtime.latest_tag_quality = live_qualities runtime.last_message_at = now runtime.detection_times.append(now) runtime.valid_flags.append(valid) + if runtime.current_required_roles != tuple( + runtime.preflight_roles + ): + runtime.task_total_frames += 1 + if valid: + runtime.task_valid_frames += 1 if not valid or selected is None: return runtime.last_valid_at = now @@ -1198,21 +2767,30 @@ class G20ThreeCameraCalibrationNode(Node): sweep_capture = bool( self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} and self.active_sweep is not None - and self.active_sweep.spec.view == view + and view in _sweep_views(_node_profile(self), self.active_sweep.spec) ) validation_capture = bool( self.state == STATE_VALIDATION_CAPTURE - and self.active_validation is not None - and self.active_validation.spec.view == view + and ( + ( + self.active_validation is not None + and view in _sweep_views( + self.profile, self.active_validation.spec + ) + ) + or self.active_combination_validation is not None + ) ) if not sweep_capture and not validation_capture: return - matched = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) + matched = matched_tracking + if matched is None: + matched = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) if matched is None: return state_u8, sync_error_ns = matched @@ -1225,7 +2803,7 @@ class G20ThreeCameraCalibrationNode(Node): if self.extrinsics is None or not runtime.extrinsics_valid: return front_from_view = self.extrinsics.transform(view) - for name, spec in self.profile.joint_specs.items(): + for name, spec in self.profile.record_specs.items(): if not spec.measured or spec.view != view: continue assert spec.parent_role is not None and spec.child_role is not None @@ -1282,7 +2860,9 @@ class G20ThreeCameraCalibrationNode(Node): def _accept_frame(self, observation: FrameObservation) -> None: if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - if observation.view != self.active_sweep.spec.view: + if observation.view not in _sweep_views( + _node_profile(self), self.active_sweep.spec + ): return if not self._motion_command_reached( self.active_sweep.spec, @@ -1293,12 +2873,14 @@ 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)) if len(self.sweep_start_frames) > maximum_start_frames: del self.sweep_start_frames[:-maximum_start_frames] return if self.state == STATE_SWEEP and self.active_sweep is not None: - if observation.view != self.active_sweep.spec.view: + if observation.view not in _sweep_views( + self.profile, self.active_sweep.spec + ): return motor = self.active_sweep.spec.motor_index value = float(observation.state_u8[motor]) @@ -1311,12 +2893,53 @@ class G20ThreeCameraCalibrationNode(Node): ): return self.sweep_frames.append(observation) + checkpoint_target = getattr( + self, "sweep_checkpoint_target_u8", None + ) + if ( + checkpoint_target is not None + and getattr(self, "sweep_checkpoint_hold_since", None) + is not None + and self._steady_checkpoint_reached( + self.active_sweep.spec, + int(checkpoint_target), + observation.state_u8, + ) + ): + self.sweep_checkpoint_frames.append(observation) + if ( + getattr(self, "sweep_baseline_pending", False) + and getattr(self, "sweep_baseline_hold_since", None) + is not None + and observation.received_at + - float(self.sweep_baseline_hold_since) + >= float(self.baseline_hold_seconds) + and self._motion_command_reached( + self.active_sweep.spec, + int( + self.baseline_command[ + self.active_sweep.spec.motor_index + ] + ), + observation.state_u8, + ) + ): + self.sweep_baseline_frames.append(observation) self.sweep_last_valid_at = observation.received_at + last_by_view = getattr( + self, "sweep_last_valid_at_by_view", None + ) + if last_by_view is None: + self.sweep_last_valid_at_by_view = {} + last_by_view = self.sweep_last_valid_at_by_view + last_by_view[observation.view] = observation.received_at return if ( self.state == STATE_VALIDATION_CAPTURE and self.active_validation is not None - and observation.view == self.active_validation.spec.view + and observation.view in _sweep_views( + self.profile, self.active_validation.spec + ) ): if self._motion_command_reached( self.active_validation.spec, @@ -1324,6 +2947,17 @@ class G20ThreeCameraCalibrationNode(Node): observation.state_u8, ): self.validation_frames_buffer.append(observation) + return + if ( + self.state == STATE_VALIDATION_CAPTURE + and self.active_combination_validation is not None + and self._command_vector_reached( + self.active_combination_validation.command_u8 + ) + ): + self.combination_validation_frames_buffer[ + observation.view + ].append(observation) def _view_ready(self, runtime: ViewRuntime, now: float) -> bool: minimum_frames = min(30, self.preflight_frames) @@ -1337,13 +2971,65 @@ class G20ThreeCameraCalibrationNode(Node): ) def _all_preflight_ready(self, now: float) -> bool: + feedback_hz = ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / (self.state_receive_times[-1] - self.state_receive_times[0]) + ) return bool( self.extrinsics is not None and len(self.latest_state_u8) == 20 and now - self.last_state_at <= 1.0 + and feedback_hz >= self.minimum_feedback_hz and all(self._view_ready(runtime, now) for runtime in self.views.values()) ) + def _all_devices_ready(self, now: float) -> bool: + """Check only hardware transport needed for a safe baseline return. + + Tag visibility must not gate this check: an interrupted calibration can + leave the fingers in a pose that occludes a fixed palm Tag. Requiring + that Tag before commanding baseline would create a startup deadlock. + """ + feedback_hz = ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / (self.state_receive_times[-1] - self.state_receive_times[0]) + ) + return bool( + self.extrinsics is not None + and len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 + and feedback_hz >= self.minimum_feedback_hz + and all( + runtime.camera_info_valid + and runtime.extrinsics_valid + and runtime.last_message_at > 0.0 + and now - runtime.last_message_at <= 1.0 + for runtime in self.views.values() + ) + ) + + def _finish_startup_baseline_recovery(self) -> None: + """Start a fresh fixed-palm-Tag window at confirmed baseline.""" + for runtime in self.views.values(): + runtime.valid_flags.clear() + runtime.detection_times.clear() + observations = getattr(runtime, "fixed_base_observations", None) + if observations is not None: + observations.clear() + runtime.locked_base_pose = None + runtime.locked_base_center_xy_px = None + runtime.locked_base_quality = None + self._reset_view_trackers(runtime) + self.startup_baseline_recovered = True + self.state = STATE_PREFLIGHT + self.reason = "waiting_for_baseline_tags_after_recovery" + def _active_view_for_resume(self) -> str | None: if self.active_sweep is not None: return self.active_sweep.spec.view @@ -1353,16 +3039,24 @@ class G20ThreeCameraCalibrationNode(Node): return self.retry_sweep_spec.view return None + def _active_views_for_resume(self) -> tuple[str, ...]: + if self.active_sweep is not None: + return _sweep_views(_node_profile(self), self.active_sweep.spec) + if self.active_validation is not None: + return _sweep_views(_node_profile(self), self.active_validation.spec) + if self.retry_sweep_spec is not None: + return _sweep_views(_node_profile(self), self.retry_sweep_spec) + return required_resume_views(None) + def _resume_preflight_ready(self, now: float) -> bool: if ( len(self.latest_state_u8) != 20 or now - self.last_state_at > 1.0 ): return False - active_view = self._active_view_for_resume() return all( self._view_ready(self.views[view], now) - for view in required_resume_views(active_view) + for view in self._active_views_for_resume() ) def _prepare_failed_sweep_retry(self) -> SweepSpec: @@ -1372,12 +3066,28 @@ class G20ThreeCameraCalibrationNode(Node): spec = self.retry_sweep_spec repetitions = int(getattr(self, "repetitions", 3)) cycles = set(getattr(self, "retry_cycles", set(range(repetitions)))) + baseline_records_by_joint = getattr( + self, "baseline_records_by_joint", {} + ) for joint_name in spec.joints: self.records_by_joint[joint_name][:] = [ record for record in self.records_by_joint[joint_name] if int(record.get("cycle", -1)) not in cycles ] + if joint_name in baseline_records_by_joint: + baseline_records_by_joint[joint_name][:] = [ + record + for record in baseline_records_by_joint[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] + command_records = getattr(self, "command_records_by_joint", {}) + if joint_name in command_records: + command_records[joint_name][:] = [ + record + for record in command_records[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] all_items = getattr(self, "sweep_items", []) self.retry_sweep_items = [ item @@ -1399,26 +3109,30 @@ class G20ThreeCameraCalibrationNode(Node): # orientation residual, so reset only the affected camera before the # retry. Raw samples remain logged and all final quality gates stay # unchanged. - runtime = getattr(self, "views", {}).get(spec.view) reset_trackers = getattr(self, "_reset_view_trackers", None) - if runtime is not None and reset_trackers is not None: + for view in _sweep_views(_node_profile(self), spec): + runtime = getattr(self, "views", {}).get(view) + if runtime is None or reset_trackers is None: + continue reset_trackers(runtime) runtime.pnp_invalid_since = None runtime.pnp_reset_count += 1 - self.sweep_attempts[spec.motor_index] = ( - self.sweep_attempts.get(spec.motor_index, 1) + 1 + storage_key = _sweep_storage_key(spec) + self.sweep_attempts[storage_key] = ( + self.sweep_attempts.get(storage_key, 1) + 1 ) for key in list(getattr(self, "sweep_retry_counts", {})): - if key[0] == spec.motor_index: + if key[0] == storage_key: self.sweep_retry_counts.pop(key, None) append_jsonl( self.raw_path, { "kind": "retry", "view": spec.view, + **({"task_name": spec.key} if spec.task_name else {}), "motor_index": spec.motor_index, "joints": list(spec.joints), - "attempt": self.sweep_attempts[spec.motor_index], + "attempt": self.sweep_attempts[storage_key], "reason": self.paused_reason, "cycles": [cycle + 1 for cycle in sorted(cycles)], }, @@ -1427,6 +3141,182 @@ class G20ThreeCameraCalibrationNode(Node): getattr(self, "motion_stall_details", {}).clear() return spec + def _restore_durable_task_checkpoint(self) -> int: + """Restore independently validated complete tasks from a failed session.""" + source = self.resume_raw_samples_path + if source is None: + return 0 + if not source.is_file(): + raise RuntimeError(f"resume raw samples do not exist: {source}") + if source.resolve() == self.raw_path.resolve(): + raise RuntimeError("resume raw samples must come from an older session") + rows: list[dict[str, Any]] = [] + with source.open("r", encoding="utf-8") as stream: + for line_number, line in enumerate(stream, start=1): + if not line.strip(): + continue + try: + value = json.loads(line) + except json.JSONDecodeError as error: + raise RuntimeError( + f"resume raw JSON is invalid at line {line_number}" + ) from error + if isinstance(value, dict): + rows.append(value) + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise RuntimeError("resume raw must contain exactly one session_start") + start = starts[0] + if ( + 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()} + or tuple(int(value) for value in start.get("baseline_command_u8", [])) + != tuple(self.baseline_command) + or str(start.get("source_urdf_sha256", "")) + != _file_sha256(self.source_urdf_path) + ): + raise RuntimeError( + "resume checkpoint geometry, Tag layout, baseline or source URDF differs" + ) + completed, reusable = resumable_completed_task_prefix( + self.profile, + self.repetitions, + self.baseline_command, + rows, + minimum_sweep_bins=self.minimum_sweep_bins, + allow_sparse=True, + ) + for durable in reusable: + kind = str(durable["kind"]) + if kind == "synchronised_frame": + continue + joint_name = str(durable["joint"]) + if joint_name not in self.records_by_joint: + raise RuntimeError( + f"resume checkpoint contains unknown joint {joint_name}" + ) + record = dict(durable) + if kind in {"sample", "baseline_hold_sample"}: + record["command_u8"] = int( + round(float(record["feedback_u8"])) + ) + elif kind == "steady_command_sample": + record["command_u8"] = int(record["requested_command_u8"]) + else: + continue + if kind == "sample": + self.records_by_joint[joint_name].append(record) + elif kind == "baseline_hold_sample": + self.baseline_records_by_joint[joint_name].append(record) + else: + self.command_records_by_joint[joint_name].append(record) + completed, dropped = G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + self, completed, allow_sparse=True + ) + completed_set = set(completed) + reusable = tuple( + row + for row in reusable + if str(row.get("task_name", "")) in completed_set + ) + self.resumed_task_keys = completed + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) + self.resume_source_session = source.parent.name + missing_tasks = [ + spec.key + for spec in self.profile.sweep_specs + if spec.key not in completed_set + ] + append_jsonl( + self.raw_path, + { + "kind": "resume_checkpoint_import", + "source_session": source.parent.name, + "completed_task_count": len(completed), + "completed_task_keys": list(completed), + "discarded_incomplete_task": ( + missing_tasks[0] if missing_tasks else None + ), + "pending_task_keys": missing_tasks, + "revalidation_dropped_tasks": dropped, + "imported_record_count": len(reusable), + "source_raw_samples_sha256": _file_sha256(source), + }, + ) + append_jsonl_many(self.raw_path, reusable) + return len(completed) + + def _revalidate_imported_tasks( + self, completed: Sequence[str], *, allow_sparse: bool = False + ) -> tuple[list[str], list[dict[str, Any]]]: + """Re-apply the final hard gates to imported tasks at import time. + + A task that only provisionally passed (warning band) in its source + session would otherwise survive until the final fit re-applies the + unmodified thresholds after every other task is collected, forcing + the session to go back and re-scan it at the very end. Failing + tasks are dropped here. Sparse product resume independently checks + later complete tasks so only the failed task has to be re-collected; + the compatibility mode still drops the remaining suffix. + """ + accepted: list[str] = [] + dropped: list[dict[str, Any]] = [] + for task_key in completed: + spec = next( + ( + item.spec + for item in self.sweep_items + if item.spec.key == task_key + ), + None, + ) + failures = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + self, spec, include_view_validity=False + ) + if spec is not None + else [{"metric": "missing_task_spec"}] + ) + if failures: + dropped.append( + { + "task": task_key, + "failures": [ + { + key: value + for key, value in failure.items() + if key in {"joint", "metric", "actual", "limit"} + } + for failure in failures[:6] + ], + } + ) + if allow_sparse: + continue + break + accepted.append(task_key) + if dropped: + accepted_set = set(accepted) + for task_key in completed: + if task_key in accepted_set: + continue + for spec in self.profile.sweep_specs: + if spec.key != task_key: + continue + for joint_name in spec.joints: + for store in ( + self.records_by_joint, + self.baseline_records_by_joint, + self.command_records_by_joint, + ): + if joint_name in store: + store[joint_name].clear() + if isinstance(completed, tuple): + accepted = tuple(accepted) + return accepted, dropped + def _start_callback( self, request: Trigger.Request, response: Trigger.Response ) -> Trigger.Response: @@ -1452,40 +3342,90 @@ class G20ThreeCameraCalibrationNode(Node): response.message = "another node is publishing hand commands" return response self.started = True - self.sweep_items = [ - SweepItem(spec, cycle, direction) - for spec in self.profile.sweep_specs - for cycle in range(self.repetitions) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) - ] + self.startup_baseline_recovered = False + self.sweep_items = [] + selected_sweep_specs = list(self.profile.sweep_specs) + if self.cross_view_roll_diagnostic_finger: + selected_sweep_specs = [ + spec + for spec in selected_sweep_specs + if spec.key + == f"{self.cross_view_roll_diagnostic_finger}_roll_multiview" + ] + if len(selected_sweep_specs) != 1: + 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, + ) + ) self.sweep_index = 0 self.retry_sweep_spec = None self.retry_resume_index = None self.retry_sweep_items.clear() self.active_sweep_is_fit_retry = False self.fit_failure = {} + self.fit_failure_history_by_task.clear() + self.cross_view_roll_diagnostic_result = {} self.motion_stall_details.clear() self.sweep_attempts = { - spec.motor_index: 1 for spec in self.profile.sweep_specs + _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs } self.sweep_retry_counts.clear() self.motion_retry_counts.clear() self.validation_retry_counts.clear() + self.precheck_speed_metrics.clear() + self.formal_speed_scales.clear() self.records_by_joint = { - name: [] for name in self.profile.measured_joints + name: [] for name in self.profile.record_joints + } + self.baseline_records_by_joint = { + name: [] for name in self.profile.record_joints + } + 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.combination_validation_items.clear() + self.combination_validation_index = 0 + self.active_combination_validation = None + self.combination_validation_completed = False + self.combination_tag_mounts.clear() + self.combination_tag_observation_counts.clear() + self.combination_tag_validation_counts.clear() + self.combination_position_errors_m.clear() + self.combination_orientation_errors_rad.clear() + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() self.completed_payload = None self.sweep_frames.clear() + self.sweep_baseline_frames.clear() + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None self.sweep_start_frames.clear() append_jsonl( self.raw_path, { "kind": "session_start", "hand_type": self.hand_type, + "tag_layout": self.profile.layout_id, "reference_finger": self.profile.reference_finger, "view_tags": { view: dict(tags) @@ -1498,11 +3438,58 @@ class G20ThreeCameraCalibrationNode(Node): ], "camera_extrinsics_file": str(self.camera_extrinsics_file), "source_urdf_path": str(self.source_urdf_path), + "source_urdf_sha256": _file_sha256(self.source_urdf_path), + "curve_input_domain": ( + "requested_command_u8" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "command_u8" + ), + "baseline_hysteresis_source": ( + "dedicated_mid_sweep_hold" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "trajectory_endpoint" + ), + "directional_zero_policy": ( + "canonical_decreasing_255_to_127" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "independent_direction_rezero" + ), + "thumb_ip_pnp_coupling": { + "usage": "candidate_prior_with_visual_fallback", + }, + "cross_view_roll_diagnostic_finger": ( + self.cross_view_roll_diagnostic_finger + ), + "training_cycles": list(range(self.repetitions - 1)), + "validation_cycle": self.repetitions - 1, + "adaptive_formal_speed": { + "enabled": bool(self.adaptive_formal_speed_enabled), + "maximum_scale": float( + self.adaptive_formal_speed_max_scale + ), + "minimum_bins": int( + self.adaptive_formal_speed_minimum_bins + ), + "maximum_bin_gap": int( + self.adaptive_formal_speed_maximum_bin_gap + ), + }, }, ) - self._begin_return_baseline("next_sweep") + try: + restored_tasks = self._restore_durable_task_checkpoint() + except Exception as error: + self.started = False + response.success = False + response.message = f"CFG-RESUME-009:{error}" + return response + self._begin_return_baseline("startup_tag_preflight") response.success = True - response.message = "three-camera calibration started" + response.message = ( + "three-camera calibration started" + if restored_tasks == 0 + else f"calibration resumed with {restored_tasks} completed tasks" + ) return response def _pause_callback( @@ -1535,16 +3522,23 @@ class G20ThreeCameraCalibrationNode(Node): "the camera/tag/URDF geometry" ) return response + if self.paused_reason == "cross_view_roll_diagnostic_complete": + response.success = False + response.message = ( + "cross-view roll diagnostic is complete and URDF publication " + "is locked; inspect the front/side result and start a formal " + "session only after choosing the mechanical or vision remedy" + ) + return response now = time.monotonic() - active_view = self._active_view_for_resume() if not self._resume_preflight_ready(now): response.success = False - required = ",".join(required_resume_views(active_view)) + required = ",".join(self._active_views_for_resume()) response.message = ( f"resume preflight is not ready; required_views={required}" ) return response - for view in required_resume_views(active_view): + for view in self._active_views_for_resume(): runtime = self.views[view] self._reset_view_trackers(runtime) runtime.pnp_invalid_since = None @@ -1562,6 +3556,24 @@ class G20ThreeCameraCalibrationNode(Node): return response if self.active_sweep is not None and self.sweep_index < len(self.sweep_items): self._begin_return_baseline("resume_sweep") + elif self.active_combination_validation is not None: + item = self.active_combination_validation + active = { + "kind": "combination_validation", + "pose_name": item.name, + "label_zh": item.label_zh, + "pose_index": self.combination_validation_index + 1, + "pose_total": len(self.combination_validation_items), + "requested_command_u8": list(item.command_u8), + "valid_frames_by_view": { + view: len(frames) + for view, frames in self.combination_validation_frames_buffer.items() + }, + "automatic_retry_count": self.validation_retry_counts.get( + ("combination", self.combination_validation_index), 0 + ), + "automatic_retry_limit": self.automatic_sweep_retry_limit, + } elif self.active_validation is not None: self._begin_return_baseline("resume_validation") else: @@ -1575,13 +3587,25 @@ class G20ThreeCameraCalibrationNode(Node): ) -> Trigger.Response: del request self._publish_speed_profile(self._normal_speed_profile()) - hold_current = getattr(self, "_publish_hold_current", None) - if hold_current is not None: - hold_current() - self.state = STATE_ABORTED - self.reason = "operator_abort" + unsafe_to_return = bool( + self.motion_stall_details + or "stall" in str(self.reason) + or len(self.latest_state_u8) != 20 + or time.monotonic() - self.last_state_at > 1.0 + ) + if unsafe_to_return: + self._publish_hold_current() + self.state = STATE_ABORTED + self.reason = "operator_abort_held_current_for_safety" + else: + self.abort_original_reason = str(self.reason or "operator_abort") + self._begin_return_baseline("abort") response.success = True - response.message = "calibration aborted and current pose held" + response.message = ( + "calibration aborted and current pose held" + if unsafe_to_return + else "calibration stopping after safe baseline return" + ) return response def _publish_command(self, values: list[int]) -> None: @@ -1616,21 +3640,80 @@ class G20ThreeCameraCalibrationNode(Node): def _normal_speed_profile(self) -> list[int]: return [self.normal_calibration_speed] * 5 - def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: + def _transition_speed_profile( + self, target_command: Sequence[int] + ) -> list[int]: + """Use conservative joint-class speeds for one safe pose waypoint.""" + speeds = list(self._normal_speed_profile()) + current = getattr(self, "latest_state_u8", ()) + if len(current) != 20 or len(target_command) != 20: + return speeds + roll_speed = int( + getattr(self, "index_roll_calibration_speed", min(speeds)) + ) + flex_speed = int( + getattr(self, "index_flex_calibration_speed", min(speeds)) + ) + for motor, (actual, target) in enumerate( + zip(current, target_command) + ): + if abs(float(actual) - float(target)) <= 0.5: + continue + if motor in range(6, 10): + slot = motor - 5 + speeds[slot] = min(speeds[slot], roll_speed) + elif motor in range(1, 5): + slot = motor + speeds[slot] = min(speeds[slot], flex_speed) + elif motor in range(16, 20): + slot = motor - 15 + speeds[slot] = min(speeds[slot], flex_speed) + return speeds + + @staticmethod + def _speed_slot_for_spec(spec: SweepSpec) -> int: + motor = int(spec.motor_index) + if motor in {0, 5, 10, 15}: + return 0 + if motor in range(1, 5): + return motor + if motor in range(6, 10): + return motor - 5 + if motor in range(16, 20): + return motor - 15 + raise ValueError(f"unsupported G20 calibration motor: {motor}") + + def _base_speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: profile = getattr(self, "profile", LEFT_HAND_PROFILE) - speeds = build_calibration_speed_profile( + return build_calibration_speed_profile( spec, normal_speed=self.normal_calibration_speed, index_roll_speed=self.index_roll_calibration_speed, index_flex_speed=self.index_flex_calibration_speed, profile=profile, ) + + def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: + speeds = G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( + self, spec + ) + active = getattr(self, "active_sweep", None) + precheck = bool(active is not None and active.precheck) + fit_retry = bool(getattr(self, "active_sweep_is_fit_retry", False)) + if not precheck and not fit_retry: + scale = float( + getattr(self, "formal_speed_scales", {}).get(spec.key, 1.0) + ) + slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(spec) + speeds[slot] = min( + 255, int(math.floor(float(speeds[slot]) * scale + 1e-9)) + ) retry = 0 - if self.active_sweep is not None: + if active is not None: key = ( - self.active_sweep.spec.motor_index, - self.active_sweep.cycle, - self.active_sweep.direction, + _sweep_storage_key(active.spec), + active.cycle, + active.direction, ) retry = self.sweep_retry_counts.get(key, 0) if retry: @@ -1641,11 +3724,140 @@ class G20ThreeCameraCalibrationNode(Node): ] return speeds + def _record_precheck_speed_metric( + self, + item: SweepItem, + *, + bin_count: int, + maximum_bin_gap: int, + valid_frames: int, + ) -> None: + """Use the accepted low-speed precheck to bound formal-scan speed.""" + key = ( + _sweep_storage_key(item.spec), item.cycle, item.direction + ) + retry = int(getattr(self, "sweep_retry_counts", {}).get(key, 0)) + base_speeds = ( + G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( + self, item.spec + ) + ) + actual_speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec( + self, item.spec + ) + slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(item.spec) + base_speed = int(base_speeds[slot]) + actual_speed = int(actual_speeds[slot]) + acquisition_scale = ( + 1.0 + if base_speed <= 0 + else float(actual_speed) / float(base_speed) + ) + metrics = getattr(self, "precheck_speed_metrics", None) + if metrics is None: + self.precheck_speed_metrics = {} + metrics = self.precheck_speed_metrics + by_direction = metrics.setdefault(item.spec.key, {}) + by_direction[item.direction] = { + "bin_count": int(bin_count), + "maximum_bin_gap": int(maximum_bin_gap), + "valid_frames": int(valid_frames), + "retry": retry, + "acquisition_speed": actual_speed, + "acquisition_speed_scale": acquisition_scale, + } + required = { + DIRECTION_DECREASING, + DIRECTION_INCREASING, + } + if not required.issubset(by_direction): + return + + enabled = bool( + getattr(self, "adaptive_formal_speed_enabled", False) + ) + # 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 + # density alone is not sufficient evidence to accelerate roll. + eligible = int(item.spec.motor_index) not in range(6, 10) + selected_scale = 1.0 + if enabled and eligible and base_speed > 0: + caps: list[float] = [] + target_bins = int(self.adaptive_formal_speed_minimum_bins) + target_gap = int(self.adaptive_formal_speed_maximum_bin_gap) + for direction in sorted(required): + metric = by_direction[direction] + observed_scale = float(metric["acquisition_speed_scale"]) + caps.append( + observed_scale + * float(metric["bin_count"]) + / float(target_bins) + ) + caps.append( + observed_scale + * float(target_gap) + / float(max(1, int(metric["maximum_bin_gap"]))) + ) + selected_scale = max( + 1.0, + min(float(self.adaptive_formal_speed_max_scale), *caps), + ) + selected_speed = min( + 255, + int(math.floor(float(base_speed) * selected_scale + 1e-9)), + ) + if selected_speed <= base_speed or base_speed <= 0: + selected_speed = base_speed + selected_scale = 1.0 + else: + selected_scale = float(selected_speed) / float(base_speed) + scales = getattr(self, "formal_speed_scales", None) + if scales is None: + self.formal_speed_scales = {} + scales = self.formal_speed_scales + scales[item.spec.key] = selected_scale + raw_path = getattr(self, "raw_path", None) + if raw_path is not None: + append_jsonl( + raw_path, + { + "kind": "task_formal_speed_selected", + "task_name": item.spec.key, + "motor_index": int(item.spec.motor_index), + "enabled": enabled, + "eligible": eligible, + "ineligible_reason": ( + "roll_baseline_hysteresis_sensitive" + if enabled and not eligible + else "" + ), + "base_speed": base_speed, + "formal_speed": selected_speed, + "formal_speed_scale": round(selected_scale, 6), + "minimum_retained_bins": int( + self.adaptive_formal_speed_minimum_bins + ), + "maximum_retained_bin_gap": int( + self.adaptive_formal_speed_maximum_bin_gap + ), + "precheck": { + direction: dict(by_direction[direction]) + for direction in sorted(required) + }, + }, + ) + def _active_endpoint_hold_seconds(self) -> float: if self.active_sweep is None: return float(self.endpoint_hold_seconds) + if self.active_sweep.precheck: + return max( + float(self.endpoint_hold_seconds), + float(self.task_precheck_hold_seconds), + ) key = ( - self.active_sweep.spec.motor_index, + _sweep_storage_key(self.active_sweep.spec), self.active_sweep.cycle, self.active_sweep.direction, ) @@ -1667,13 +3879,59 @@ class G20ThreeCameraCalibrationNode(Node): ] self._publish_command(values) + def _snap_reached_state_to_command( + self, + current_state: Sequence[float], + target_command: Sequence[int], + ) -> tuple[int, ...]: + """Remove no-op transition steps already inside feedback tolerance.""" + if len(current_state) != 20 or len(target_command) != 20: + raise ValueError("state and target command must contain 20 values") + result = [ + int(np.clip(np.rint(value), 0, 255)) for value in current_state + ] + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + for index in controlled: + target = int(target_command[index]) + if abs(float(current_state[index]) - target) <= ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, target + ) + ): + result[index] = target + return tuple(result) + def _begin_return_baseline(self, after: str) -> None: self.baseline_after = str(after) - self.return_command_u8 = ( + target_command = ( G20ThreeCameraCalibrationNode._return_command_for_transition( self, after ) ) + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + current_state = target_command + else: + current_state = self._snap_reached_state_to_command( + current_state, target_command + ) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + self.return_waypoints = deque( + build_calibration_return_waypoints( + target_command, + current_command=current_state, + profile=profile, + anchor_roll_motor=( + G20ThreeCameraCalibrationNode._return_anchor_roll_motor( + self + ) + ), + parallel=self.parallel_pose_transitions, + ) + ) + self.return_command_u8 = self.return_waypoints.popleft() self.position_hold_since = None self.motion_stage_started_at = time.monotonic() getattr(self, "motion_stall_details", {}).clear() @@ -1681,19 +3939,138 @@ class G20ThreeCameraCalibrationNode(Node): self.motion_stage_started_at, self._baseline_error_u8() ) self.state = STATE_RETURN_BASELINE - self.reason = f"return_baseline_before_{after}" - self._publish_speed_profile(self._normal_speed_profile()) + self.reason = ( + "holding_same_finger_clearance_before_next_task" + if str(after) == "next_task_same_finger" + else f"return_baseline_before_{after}" + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.return_command_u8 + ) + ) self._publish_command(list(self.return_command_u8)) + def _return_anchor_roll_motor(self) -> int | None: + """Resolve the finger that must leave a fan pose first.""" + retry_items = getattr(self, "retry_sweep_items", []) + item = retry_items[0] if retry_items else None + if item is None: + items = getattr(self, "sweep_items", []) + index = int(getattr(self, "sweep_index", len(items))) + if index < len(items): + item = items[index] + if item is None: + item = getattr(self, "active_sweep", None) + if item is None: + return None + motor = int(item.spec.motor_index) + if motor in range(6, 10): + return motor + if motor in range(1, 5): + return motor + 5 + if motor in range(16, 20): + return motor - 10 + return None + + @staticmethod + def _four_finger_task_group(spec: SweepSpec) -> str | None: + """Return the finger whose consecutive tasks share one avoidance pose.""" + for joint_name in spec.joints: + finger = str(joint_name).split("_", 1)[0] + if finger in {"pinky", "ring", "middle", "index"}: + return finger + return None + + def _next_pending_sweep_item(self) -> SweepItem | None: + retry_items = getattr(self, "retry_sweep_items", []) + if retry_items: + return retry_items[0] + items = getattr(self, "sweep_items", []) + index = int(getattr(self, "sweep_index", len(items))) + return items[index] if index < len(items) else None + + def _advance_past_resumed_sweeps(self) -> None: + """Skip every independently revalidated task restored from disk.""" + completed = set(getattr(self, "resumed_task_keys", ()) or ()) + items = getattr(self, "sweep_items", []) + while ( + self.sweep_index < len(items) + and items[self.sweep_index].spec.key in completed + ): + self.sweep_index += 1 + + def _transition_after_completed_spec(self, spec: SweepSpec) -> str: + """Keep clearance parked between consecutive tasks of one finger.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item(self) + current_group = ( + G20ThreeCameraCalibrationNode._four_finger_task_group(spec) + ) + next_group = ( + None + if next_item is None + else G20ThreeCameraCalibrationNode._four_finger_task_group( + next_item.spec + ) + ) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and current_group is not None + and current_group == next_group + ): + return "next_task_same_finger" + return "next_sweep" + def _return_command_for_transition(self, after: str) -> tuple[int, ...]: - """Keep motor 5 at clearance while a motor-10 sweep is recovered.""" + """Choose a global or task-local safe transition target.""" + if str(after) in {"next_cycle", "next_task_same_finger"}: + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( + self + ) + if next_item is None: + return tuple(self.baseline_command) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return tuple( + build_calibration_motion_command( + next_item.spec, + int(self.baseline_command[next_item.spec.motor_index]), + baseline=self.baseline_command, + profile=profile, + ) + ) + if str(after) == "next_sweep": + merged = ( + G20ThreeCameraCalibrationNode._cross_group_return_command( + self + ) + ) + if merged is not None: + return merged if str(after) not in {"retry_sweep", "resume_sweep"}: return tuple(self.baseline_command) retry_items = getattr(self, "retry_sweep_items", []) item = retry_items[0] if retry_items else getattr(self, "active_sweep", None) - if item is None or item.spec.motor_index != 10: + if item is None: return tuple(self.baseline_command) profile = getattr(self, "profile", LEFT_HAND_PROFILE) + # A four-finger retry immediately repeats the same task. Preserve its + # reviewed occlusion-clearance pose and return only to the queued + # direction start; a global baseline would unfold the parked fingers + # and make the next preparation flex them again. The retry setup has + # already reset every contributing PnP tracker. + keep_task_clearance = bool( + item.spec.motor_index == 10 + or ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and G20ThreeCameraCalibrationNode._four_finger_task_group( + item.spec + ) + is not None + ) + ) + if not keep_task_clearance: + return tuple(self.baseline_command) return tuple( build_calibration_motion_command( item.spec, @@ -1703,6 +4080,65 @@ class G20ThreeCameraCalibrationNode(Node): ) ) + def _cross_group_return_command(self) -> tuple[int, ...] | None: + """Keep already-positioned clearance motors through a finger change. + + Crossing from one finger's task group to the next used to unfold every + auxiliary motor back to baseline and then re-flex it for the next + avoidance pose. Motors the next group still wants at a non-baseline + value and that are already in place (within their endpoint deadband) + now simply stay put; everything else still returns to baseline, so the + reviewed roll-before-unfold and open-before-flex orderings keep + holding and only genuinely unused clearance unfolds. + """ + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return None + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( + self + ) + if next_item is None: + return None + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + return None + baseline = [int(value) for value in self.baseline_command] + # Match what the next preparation will actually command: the measured + # motor enters at its sweep start, not at the baseline value. + next_start = int( + getattr( + next_item, + "start_u8", + baseline[int(next_item.spec.motor_index)], + ) + ) + next_final = [ + int(value) + for value in build_calibration_motion_command( + next_item.spec, + next_start, + baseline=baseline, + profile=profile, + ) + ] + controlled = { + int(spec.motor_index) for spec in profile.joint_specs.values() + } + target = list(baseline) + for motor in sorted(controlled): + next_value = next_final[motor] + if next_value == baseline[motor]: + continue + actual = float(current_state[motor]) + tolerance = ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, motor, next_value + ) + ) + if abs(actual - next_value) <= tolerance: + target[motor] = next_value + return tuple(target) + def _current_return_command(self) -> tuple[int, ...]: command = getattr(self, "return_command_u8", self.baseline_command) return tuple(int(value) for value in command) @@ -1789,6 +4225,77 @@ class G20ThreeCameraCalibrationNode(Node): for index in indices ) + def _command_vector_reached(self, command: Sequence[int]) -> bool: + if len(self.latest_state_u8) != 20 or len(command) != 20: + return False + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + return all( + abs(float(self.latest_state_u8[index]) - float(command[index])) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(command[index]) + ) + for index in controlled + ) + + def _command_vector_error_u8(self, command: Sequence[int]) -> float: + if len(self.latest_state_u8) != 20 or len(command) != 20: + return float("inf") + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + return max( + abs(float(self.latest_state_u8[index]) - float(command[index])) + for index in controlled + ) + + def _command_vector_error_details( + self, command: Sequence[int], context: str + ) -> dict[str, float | int | str]: + """Identify the actual controlled motor blocking one safe waypoint.""" + if len(self.latest_state_u8) != 20 or len(command) != 20: + return { + "stage": str(context), + "motor_index": -1, + "target_u8": float("nan"), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + controlled = sorted( + {int(spec.motor_index) for spec in self.profile.joint_specs.values()} + ) + values: list[dict[str, float | int | str]] = [] + for index in controlled: + target = float(command[index]) + actual = float(self.latest_state_u8[index]) + tolerance = G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + values.append( + { + "stage": str(context), + "motor_index": index, + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": float(tolerance), + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + return max( + outside or values, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + def _motion_command_reached( self, spec: SweepSpec, @@ -1820,6 +4327,65 @@ class G20ThreeCameraCalibrationNode(Node): for index in indices ) + def _steady_checkpoint_reached( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> bool: + """Accept a settled command/feedback offset without hiding a stall. + + The checkpoint exists specifically to identify requested-command to + feedback/angle offsets, so applying the ordinary ±2-u8 equality gate + here is circular. Only the swept motor gets the bounded calibration + deadband; every clearance motor keeps its normal strict tolerance. + """ + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return False + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + swept_tolerance = max( + float(self.steady_checkpoint_command_feedback_tolerance_u8), + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + self, spec, command_u8 + ), + ) + if ( + abs(float(state[spec.motor_index]) - expected[spec.motor_index]) + > swept_tolerance + ): + return False + return all( + abs(float(state[index]) - expected[index]) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, expected[index] + ) + for index in calibration_auxiliary_commands( + spec, profile=self.profile + ) + ) + + def _steady_checkpoint_feedback_is_stable( + self, + spec: SweepSpec, + frames: Sequence[FrameObservation], + ) -> bool: + """Require the held feedback range to be small before recording it.""" + if len(frames) < 3: + return False + values = [ + float(frame.state_u8[spec.motor_index]) for frame in frames + ] + return bool( + max(values) - min(values) + <= float(self.steady_checkpoint_maximum_feedback_range_u8) + ) + def _motion_command_error_u8( self, spec: SweepSpec, @@ -1934,6 +4500,15 @@ class G20ThreeCameraCalibrationNode(Node): if reference - error >= minimum_progress: self._reset_motion_progress(now, error) return False + motion_started_value = getattr(self, "motion_stage_started_at", None) + startup_grace = float( + getattr(self, "motor_stall_startup_grace_seconds", 1.0) + ) + if ( + motion_started_value is not None + and now - float(motion_started_value) < startup_grace + ): + return False timeout = float(getattr(self, "motor_stall_timeout_seconds", 8.0)) if now - last_progress < timeout: return False @@ -1942,12 +4517,14 @@ class G20ThreeCameraCalibrationNode(Node): for key in ("motor_index", "target_u8", "actual_u8", "tolerance_u8"): if key in detail_payload: reason_parts.append(f"{key}={detail_payload[key]}") + reason_parts.append(f"timeout_seconds={timeout:.3f}") reason_parts.append(f"error_u8={error:.3f}") reason = ":".join(reason_parts) self.motion_stall_details = { "kind": "motion_stall", "stage": str(context), "error_u8": error, + "timeout_seconds": timeout, **detail_payload, } append_jsonl( @@ -1970,7 +4547,7 @@ class G20ThreeCameraCalibrationNode(Node): if self.active_sweep is None: return float(self.sweep_timeout_seconds) key = ( - self.active_sweep.spec.motor_index, + _sweep_storage_key(self.active_sweep.spec), self.active_sweep.cycle, self.active_sweep.direction, ) @@ -2031,30 +4608,72 @@ class G20ThreeCameraCalibrationNode(Node): 5.0, ) ) + if ( + profile.side == "right" + and int(motor_index) == 19 + and int(endpoint_u8) == 0 + ): + return float( + getattr( + self, + "pinky_pip_zero_endpoint_tolerance_u8", + 5.0, + ) + ) return float(self.endpoint_tolerance_u8) + def _requires_mid_sweep_baseline_hold(self, item: SweepItem) -> bool: + """Return whether this formal sweep crosses a non-endpoint zero. + + The four MCP-roll motors use command 127 as zero. Their zero-pose + hysteresis must be measured after settling at 127 from each direction, + not inferred from frames captured while the motor is moving through + 127. Endpoint-zero joints already have static start/end holds. + """ + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + or item.precheck + or item.cycle < 0 + ): + return False + baseline = int(self.baseline_command[item.spec.motor_index]) + return baseline not in {item.start_u8, item.target_u8} + def _start_next_sweep(self) -> None: retry_items = getattr(self, "retry_sweep_items", []) if retry_items: self.active_sweep = retry_items.pop(0) self.active_sweep_is_fit_retry = True - elif self.sweep_index >= len(self.sweep_items): - self.active_sweep = None - self._begin_return_baseline("fit") - return else: + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) + if self.sweep_index >= len(self.sweep_items): + self.active_sweep = None + self._begin_return_baseline("fit") + return 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: - runtime = getattr(self, "views", {}).get(item.spec.view) reset_trackers = getattr(self, "_reset_view_trackers", None) - if runtime is not None and reset_trackers is not None: - # Make every cycle an independent PnP-branch observation. - # The hand is still at the start endpoint, so the group - # tracker receives its full static initialization window - # before _accept_frame allows the sweep to start. - reset_trackers(runtime) + preserve_task_reference = bool( + not item.precheck and item.cycle > 0 + ) + for view in _sweep_views(_node_profile(self), item.spec): + 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. + if preserve_task_reference: + reset_trackers( + runtime, preserve_task_reference=True + ) + else: + reset_trackers(runtime) runtime.pnp_invalid_since = None runtime.pnp_reset_count += 1 raw_path = getattr(self, "raw_path", None) @@ -2063,42 +4682,111 @@ class G20ThreeCameraCalibrationNode(Node): raw_path, { "kind": "pnp_cycle_initialization", - "view": item.spec.view, + "view": view, "motor_index": item.spec.motor_index, "cycle": item.cycle, + "task_reference_preserved": ( + preserve_task_reference + ), }, ) getattr(self, "sweep_frames", []).clear() + getattr(self, "sweep_baseline_frames", []).clear() + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None getattr(self, "sweep_start_frames", []).clear() self.position_hold_since = None self.motion_stage_started_at = time.monotonic() + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + current_state = self.baseline_command + else: + preparation_target = build_calibration_motion_command( + item.spec, + item.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + current_state = self._snap_reached_state_to_command( + current_state, preparation_target + ) + self.preparation_waypoints = deque( + build_calibration_preparation_waypoints( + item.spec, + item.start_u8, + current_command=current_state, + baseline=self.baseline_command, + profile=self.profile, + parallel=self.parallel_pose_transitions, + ) + ) + self.preparation_command_u8 = self.preparation_waypoints.popleft() self._reset_motion_progress( self.motion_stage_started_at, - self._motion_command_error_u8(item.spec, item.start_u8), + self._command_vector_error_u8(self.preparation_command_u8), ) self.state = STATE_PREPARE_SWEEP self.reason = ( f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" f"cycle_{item.cycle}_{item.direction}" ) - self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) - self._publish_command( - build_calibration_motion_command( - item.spec, - item.start_u8, - baseline=self.baseline_command, - profile=self.profile, + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.preparation_command_u8 ) ) + self._publish_command(list(self.preparation_command_u8)) def _begin_active_sweep(self, now: float) -> None: assert self.active_sweep is not None self.sweep_frames.clear() self.sweep_frames.extend(self.sweep_start_frames) self.sweep_start_frames.clear() + if not hasattr(self, "sweep_baseline_frames"): + self.sweep_baseline_frames = [] + self.sweep_baseline_frames.clear() + self.sweep_baseline_pending = ( + G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( + self, self.active_sweep + ) + ) + self.sweep_baseline_hold_since = None self.sweep_started_at = now self.sweep_last_valid_at = now + capture_views = _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + self.sweep_last_valid_at_by_view = { + view: now for view in capture_views + } self.sweep_endpoint_since = None + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view = { + view: 0 for view in capture_views + } + self.sweep_detection_valid_by_view = { + view: 0 for view in capture_views + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + self.sweep_checkpoint_commands = deque( + _steady_checkpoint_commands(profile, self.active_sweep) + ) + self.sweep_checkpoint_mode = bool(self.sweep_checkpoint_commands) + self.sweep_checkpoint_target_u8 = None + self.sweep_checkpoint_hold_since = None + if not hasattr(self, "sweep_checkpoint_frames"): + self.sweep_checkpoint_frames = [] + self.sweep_checkpoint_frames.clear() + if self.sweep_checkpoint_commands: + # PREPARE_SWEEP already held the start endpoint long enough to be + # a steady command-domain observation. + if hasattr(self, "command_records_by_joint"): + self._record_command_checkpoint( + self.active_sweep, + self.active_sweep.start_u8, + list(self.sweep_frames), + ) self._reset_motion_progress( now, self._motion_command_error_u8( @@ -2106,17 +4794,154 @@ class G20ThreeCameraCalibrationNode(Node): ), ) self.state = STATE_SWEEP - self.reason = "collecting_timestamp_synchronised_tag_centres" + self.reason = ( + "collecting_dedicated_baseline_hold" + if self.sweep_baseline_pending + else "collecting_timestamp_synchronised_tag_centres" + ) profile = getattr(self, "profile", LEFT_HAND_PROFILE) + target_u8 = ( + int(self.baseline_command[self.active_sweep.spec.motor_index]) + if self.sweep_baseline_pending + else ( + self.sweep_checkpoint_commands.popleft() + if self.sweep_checkpoint_commands + else self.active_sweep.target_u8 + ) + ) + if not self.sweep_baseline_pending and self.sweep_checkpoint_mode: + self.sweep_checkpoint_target_u8 = int(target_u8) self._publish_command( build_calibration_motion_command( self.active_sweep.spec, - self.active_sweep.target_u8, + target_u8, baseline=self.baseline_command, profile=profile, ) ) + def _record_command_checkpoint( + self, + item: SweepItem, + requested_command_u8: int, + frames: Sequence[FrameObservation], + ) -> None: + """Persist one steady requested-command/feedback/vision observation.""" + motor = int(item.spec.motor_index) + for joint_name in item.spec.joints: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < 3: + raise RuntimeError( + f"steady command checkpoint {joint_name} has fewer than 3 frames" + ) + feedback = float( + np.median([float(frame.state_u8[motor]) for frame in selected]) + ) + state = np.median( + np.asarray( + [frame.state_u8 for frame in selected], dtype=float + ), + axis=0, + ) + record = { + # command_u8 is retained only in memory for the established + # pure fitting API. The durable record below uses explicit + # requested_command_u8 and feedback_u8 names. + "command_u8": int(requested_command_u8), + "kind": "steady_command_sample", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "requested_command_u8": int(requested_command_u8), + "feedback_u8": round(feedback, 6), + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in selected]) + ), + "relative_quaternion_xyzw": [ + float(value) + for value in robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in selected + ] + )[0] + ], + "relative_translation_xyz_m": [ + float(value) + for value in np.median( + np.asarray( + [ + frame.joint_vectors_xyz_m[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + ], + "image_relative_xy_px": [ + float(value) + for value in np.median( + np.asarray( + [ + frame.image_vectors_xy_px[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + ], + "parent_pose_common": _robust_pose_payload( + [frame.parent_poses_common[joint_name] for frame in selected] + ), + "child_pose_common": _robust_pose_payload( + [frame.child_poses_common[joint_name] for frame in selected] + ), + "state_u8": [float(value) for value in state], + "valid_frames": len(selected), + } + self.command_records_by_joint[joint_name].append(record) + durable = {key: value for key, value in record.items() if key != "command_u8"} + append_jsonl(self.raw_path, durable) + + def _publish_next_checkpoint(self, now: float) -> bool: + """Advance a first-round steady scan; return False at its endpoint.""" + if not self.sweep_checkpoint_commands: + self.sweep_checkpoint_target_u8 = None + # The final checkpoint is also the normal sweep endpoint. Leave + # checkpoint mode here so _advance can run its common endpoint + # hold/completion path on this and subsequent timer ticks. Keeping + # the mode latched made the next tick treat the intentionally + # cleared target as an internal error. + self.sweep_checkpoint_mode = False + return False + assert self.active_sweep is not None + target = int(self.sweep_checkpoint_commands.popleft()) + self.sweep_checkpoint_target_u8 = target + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + self.sweep_endpoint_since = None + self.motion_stage_started_at = now + self._reset_motion_progress( + now, self._motion_command_error_u8(self.active_sweep.spec, target) + ) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + target, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + return True + def _sweep_spec_start_index(self, spec: SweepSpec) -> int: return next( index @@ -2135,20 +4960,100 @@ class G20ThreeCameraCalibrationNode(Node): profile = getattr(self, "profile", LEFT_HAND_PROFILE) zero_command = int( self.baseline_command[ - profile.joint_specs[joint_name].motor_index + profile.record_specs[joint_name].motor_index ] ) + model_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, joint_name, records + ) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + ): + return fit_joint_image_curve( + model_records, + maximum_radial_rms_px=( + self.image_trajectory_maximum_radial_rms_px + ), + maximum_radial_p95_px=( + self.image_trajectory_maximum_radial_p95_px + ), + minimum_radius_px=self.image_trajectory_minimum_radius_px, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + ) return fit_rotation_joint_curve( - records, zero_command_u8=zero_command + model_records, + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), ) - def _fit_axis_measurement( + def _hysteresis_axis_for_fit( + self, + joint_name: str, + records: Sequence[Mapping[str, Any]], + fit: JointCurveFit, + ) -> Sequence[float]: + """Return a physical rotation axis for directional zero checks. + + End-on finger flexion uses its much more stable projected circle for + the dynamic curve. Baseline hysteresis is still a pose-domain check, + so fit a separate diagnostic rotation axis instead of accidentally + treating the image circle as a 3-D pose model. + """ + axis = fit.circle.get("axis_xyz") + if axis is not None: + return axis + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_command = int( + self.baseline_command[ + profile.record_specs[joint_name].motor_index + ] + ) + diagnostic_fit = fit_rotation_joint_curve( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, joint_name, records + ), + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), + ) + return diagnostic_fit.circle["axis_xyz"] + + def _records_with_baseline_holds( + self, + joint_name: str, + records: Sequence[Mapping[str, Any]], + ) -> list[dict[str, Any]]: + """Add settled mid-range zero poses to directional model fitting.""" + samples = [dict(record) for record in records] + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if canonical_zero_direction(profile, joint_name) is None: + return samples + included_groups = { + (int(record["cycle"]), str(record["direction"])) + for record in samples + } + holds = getattr(self, "baseline_records_by_joint", {}).get( + joint_name, () + ) + samples.extend( + dict(record) + for record in holds + if ( + int(record["cycle"]), str(record["direction"]) + ) in included_groups + ) + return samples + + def _fit_axis_measurement_raw( self, joint_name: str, cycle: int ) -> JointAxisMeasurement: constraint: Sequence[float] | None = None profile = getattr(self, "profile", LEFT_HAND_PROFILE) zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - reference = profile.reference_finger upstream_joint = { # These neighbouring axes are parallel in the fixed source URDF. # A small planar Tag's monocular PnP orientation can have a stable @@ -2158,17 +5063,27 @@ class G20ThreeCameraCalibrationNode(Node): # valid only because the source-URDF axes are deliberately locked. "thumb_mcp": "thumb_cmc_pitch", "thumb_ip": "thumb_mcp", - f"{reference}_pip": f"{reference}_mcp_pitch", - f"{reference}_dip": f"{reference}_pip", + **{ + f"{finger}_pip": f"{finger}_mcp_pitch" + for finger in ("index", "middle", "ring", "pinky") + }, + **{ + f"{finger}_dip": f"{finger}_pip" + for finger in ("index", "middle", "ring", "pinky") + }, }.get(joint_name) 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 # direction), rather than reintroducing the raw PnP orientation at # the last passive joint. - upstream = self._fit_axis_measurement(upstream_joint, cycle) + upstream = ( + G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, upstream_joint, cycle + ) + ) constraint = upstream.axis_common_xyz - spec = profile.joint_specs[joint_name] + spec = profile.record_specs[joint_name] extrinsics = getattr(self, "extrinsics", None) view_normal = None if extrinsics is not None: @@ -2176,29 +5091,48 @@ class G20ThreeCameraCalibrationNode(Node): view_normal = view_transform[:3, :3] @ np.asarray( [0.0, 0.0, 1.0], dtype=float ) + measurement_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + joint_name, + self.records_by_joint[joint_name], + ) + ) measurement = fit_joint_axis_measurement( joint_name, - self.records_by_joint[joint_name], + measurement_records, cycle=cycle, zero_command_u8=int( self.baseline_command[spec.motor_index] ), axis_common_constraint=constraint, - constrained_circle_joints=zero_profile.constrained_circle_joints, + constrained_circle_joints=( + zero_profile.constrained_circle_joints + | ({joint_name} if joint_name.endswith("_side") else set()) + ), view_normal_common_xyz=view_normal, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), + ) + sweep_spec = next( + sweep + for sweep in profile.sweep_specs + if joint_name in sweep.joints ) condition_command = build_calibration_motion_command( - spec, + sweep_spec, int(self.baseline_command[spec.motor_index]), baseline=self.baseline_command, profile=profile, ) - measurement = replace( - measurement, - condition_command_u8=tuple( - float(value) for value in condition_command - ), - ) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + measurement = replace( + measurement, + condition_command_u8=tuple( + float(value) for value in condition_command + ), + ) if extrinsics is None: return measurement return replace( @@ -2206,27 +5140,320 @@ class G20ThreeCameraCalibrationNode(Node): view_normal_common_xyz=tuple(float(value) for value in view_normal), ) + def _fit_axis_measurement( + self, joint_name: str, cycle: int + ) -> JointAxisMeasurement: + """Fit one axis, fusing a separately accepted cross-view estimate.""" + primary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, joint_name, cycle + ) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + validation_sources = profile.axis_validation_sources or {} + validation_name = validation_sources.get(joint_name) + if validation_name is None: + return primary + validation_records = self.records_by_joint.get(validation_name, []) + if not any( + int(record.get("cycle", -1)) == int(cycle) + for record in validation_records + ): + # Front roll is provisionally checked before the independent side + # task has been acquired. Fusion is allowed only in the final fit + # after both complete raw datasets have independently passed. + return primary + secondary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, validation_name, cycle + ) + primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) + secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) + if float(primary_axis @ secondary_axis) < 0.0: + secondary_axis = -secondary_axis + axis_difference = math.acos( + float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) + ) + primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) + secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) + line_distance = float( + np.linalg.norm( + np.cross(secondary_point - primary_point, primary_axis) + ) + ) + # 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 + # reprojection cannot expose. Session 20260820_105535 showed a + # stable 11.4 deg front/side disagreement from exactly this effect, + # 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. + 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 + ): + raise ValueError( + "cross_view_roll_axis_gross_disagreement:" + f"{math.degrees(axis_difference):.6f}deg," + f"{1000.0 * line_distance:.6f}mm" + ) + if ( + axis_difference > self.zero_maximum_axis_cycle_difference_rad + or line_distance > self.axis_maximum_pose_line_rms_m + ): + selected_direction = primary + decision = "skip_fusion_use_primary" + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) + observer_name = next( + ( + observer + for observer, parent in zero_profile.axis_parent_joint.items() + if parent == joint_name + ), + None, + ) + observer_records = ( + [] + 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 + ) + ) + model = UrdfKinematicModel(self.source_urdf_path) + parent_axis, _ = model.axis_line( + joint_name, zero_offsets={}, joint_angles={} + ) + observer_axis, _ = model.axis_line( + observer_name, zero_offsets={}, joint_angles={} + ) + expected_cone = math.acos( + abs( + float( + np.clip(parent_axis @ observer_axis, -1.0, 1.0) + ) + ) + ) + measured_observer_axis = np.asarray( + observer.axis_common_xyz, dtype=float + ) + + def cone_residual(candidate_axis: np.ndarray) -> float: + measured_cone = math.acos( + abs( + float( + np.clip( + candidate_axis @ measured_observer_axis, + -1.0, + 1.0, + ) + ) + ) + ) + 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 + ): + selected_direction = replace( + primary, + axis_common_xyz=tuple( + float(value) for value in secondary_axis + ), + axis_direction_source=( + "cross_view_cone_selected_secondary" + ), + ) + decision = "use_secondary_zero_invariant_cone" + 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, + ) + # The front roll link rides the splay screw: its orientation + # 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. + return replace( + selected_direction, + point_common_xyz_m=tuple( + float(value) + for value in secondary.point_common_xyz_m + ), + axis_point_source="side_circle_cross_view", + pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + ) + primary_variance = max( + primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + secondary_variance = max( + secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + primary_weight = 1.0 / primary_variance + secondary_weight = 1.0 / secondary_variance + total_weight = primary_weight + secondary_weight + fused_axis = ( + primary_weight * primary_axis + + secondary_weight * secondary_axis + ) + fused_axis /= np.linalg.norm(fused_axis) + # Axis-line points have a free coordinate along the axis. Fuse only + # the observable perpendicular displacement and retain the primary + # point's along-axis gauge. + delta = secondary_point - primary_point + perpendicular_delta = delta - fused_axis * float(delta @ fused_axis) + fused_point = ( + primary_point + + secondary_weight / total_weight * perpendicular_delta + ) + return replace( + primary, + axis_common_xyz=tuple(float(value) for value in fused_axis), + point_common_xyz_m=tuple(float(value) for value in fused_point), + plane_rms_m=max(primary.plane_rms_m, secondary.plane_rms_m), + radial_rms_m=max(primary.radial_rms_m, secondary.radial_rms_m), + pose_axis_line_rms_m=max( + primary.pose_axis_line_rms_m, + secondary.pose_axis_line_rms_m, + line_distance, + ), + axis_direction_source="cross_view_weighted_fusion", + ) + + def _record_cross_view_roll_axis_diagnostic( + self, + joint_name: str, + cycle: int, + *, + primary: Any, + secondary: Any, + axis_difference_rad: float, + line_distance_m: float, + decision: str = "skip_fusion_use_primary", + ) -> None: + """Log the deterministic cross-view direction decision.""" + record = { + "kind": "cross_view_roll_axis_diagnostic", + "joint": joint_name, + "cycle": int(cycle), + "decision": str(decision), + "axis_difference_deg": round( + math.degrees(axis_difference_rad), 6 + ), + "line_distance_mm": round(1000.0 * line_distance_m, 6), + "primary_axis_common_xyz": [ + round(float(value), 9) for value in primary.axis_common_xyz + ], + "secondary_axis_common_xyz": [ + round(float(value), 9) for value in secondary.axis_common_xyz + ], + "primary_axis_direction_source": primary.axis_direction_source, + } + try: + append_jsonl(self.raw_path, record) + except Exception as error: + logger_factory = getattr(self, "get_logger", None) + if callable(logger_factory): + logger_factory().warning( + f"failed to record cross-view roll diagnostic: {error}" + ) + logger_factory = getattr(self, "get_logger", None) + if callable(logger_factory): + selected_text = ( + "using the side axis selected by the zero-invariant cone" + if decision == "use_secondary_zero_invariant_cone" + else "keeping the front-only axis" + ) + logger_factory().warning( + f"{joint_name} cycle {cycle}: front/side roll axes disagree " + f"by {math.degrees(axis_difference_rad):.2f} deg; skipping " + f"fusion and {selected_text}" + ) + def _provisional_fit_failures( - self, spec: SweepSpec + self, spec: SweepSpec, *, include_view_validity: bool = True ) -> list[dict[str, Any]]: """Check full-pose curve and 3-D axis after one six-way task.""" profile = getattr(self, "profile", LEFT_HAND_PROFILE) zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) failures: list[dict[str, Any]] = [] - if hasattr(self, "views"): - valid_rate = float(self.views[spec.view].valid_rate) - if valid_rate < self.minimum_detection_rate: - failures.append( - { - "joint": spec.joints[0], - "metric": "tag_valid_rate_percent", - "actual": round(100.0 * valid_rate, 3), - "limit": round( - 100.0 * self.minimum_detection_rate, 3 - ), - "comparison": "minimum", - } + if include_view_validity and hasattr(self, "views"): + for view in _sweep_views(profile, spec): + runtime = self.views[view] + valid_rate = float(runtime.valid_rate) + # Prefer the task-scoped counters: after the last sweep the + # required-role set flips back to the full preflight set and + # the rolling window restarts on idle frames, which is not + # what a per-task capture-quality gate should measure. + total = int(getattr(runtime, "task_total_frames", 0)) + if total > 0: + valid_rate = ( + float(getattr(runtime, "task_valid_frames", 0)) + / total + ) + imported_without_capture = ( + total == 0 + and spec.key + in set(getattr(self, "resumed_task_keys", ()) or ()) ) + if ( + not imported_without_capture + and valid_rate < self.minimum_detection_rate + ): + view_joints = _sweep_joints_for_view( + profile, spec, view + ) + failures.append( + { + "joint": view_joints[0], + "view": view, + "metric": "tag_valid_rate_percent", + "actual": round(100.0 * valid_rate, 3), + "limit": round( + 100.0 * self.minimum_detection_rate, 3 + ), + "comparison": "minimum", + "task_valid_frames": int( + getattr(runtime, "task_valid_frames", 0) + ), + "task_total_frames": total, + } + ) for joint_name in spec.joints: records = self.records_by_joint[joint_name] sync_p95 = float( @@ -2266,27 +5493,243 @@ class G20ThreeCameraCalibrationNode(Node): } ) continue - checks = ( - ( - "rotation_orthogonal_rms_deg", - math.degrees( - float(fit.quality["rotation_orthogonal_rms_rad"]) + if profile.layout_id == G20_RIGHT_19_LAYOUT: + command_store = getattr( + self, "command_records_by_joint", None + ) + if command_store is not None: + command_records = [ + dict(record) + for record in command_store.get(joint_name, ()) + if int(record.get("cycle", 0)) == 0 + ] + if len(command_records) < 18: + failures.append( + { + "joint": joint_name, + "metric": "steady_command_checkpoints", + "actual": len(command_records), + "limit": 18, + "comparison": "minimum", + } + ) + else: + try: + self._fit_joint_records( + joint_name, command_records + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "command_trajectory_fit", + "reason": str(error), + } + ) + else: + feedback_fit = self._fit_joint_records( + joint_name, + _steady_records_in_feedback_domain( + command_records + ), + ) + command_gap = float( + feedback_fit.maximum_hysteresis_rad + ) + command_gap_limit = float( + self.command_maximum_direction_gap_rad + ) + if command_gap > command_gap_limit: + failures.append( + { + "joint": joint_name, + "metric": ( + "feedback_direction_gap_deg" + ), + "actual": round( + math.degrees(command_gap), 6 + ), + "limit": round( + math.degrees(command_gap_limit), 6 + ), + "comparison": "maximum", + } + ) + try: + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, joint_name, records + ), + zero_command_u8=int( + self.baseline_command[ + profile.record_specs[joint_name].motor_index + ] + ), + axis_xyz=( + G20ThreeCameraCalibrationNode + ._hysteresis_axis_for_fit( + self, joint_name, records, fit + ) + ), + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "baseline_hysteresis", + "reason": str(error), + } + ) + continue + maximum_baseline_hysteresis = max(baseline_hysteresis) + canonical_direction = canonical_zero_direction( + profile, joint_name + ) + if canonical_direction is not None: + branch_gap_range = ( + maximum_baseline_hysteresis + - min(baseline_hysteresis) + ) + branch_gap_limit = getattr( + self, + "directional_zero_maximum_branch_gap_rad", + math.radians(1.5), + ) + branch_gap_range_limit = getattr( + self, + "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( + { + "joint": joint_name, + "metric": "baseline_directional_gap_deg", + "actual": round( + math.degrees( + maximum_baseline_hysteresis + ), + 6, + ), + "limit": round( + math.degrees(branch_gap_limit), 6 + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + if branch_gap_range > branch_gap_range_limit: + failures.append( + { + "joint": joint_name, + "metric": ( + "baseline_directional_gap_range_deg" + ), + "actual": round( + math.degrees(branch_gap_range), 6 + ), + "limit": round( + math.degrees(branch_gap_range_limit), 6 + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + elif ( + maximum_baseline_hysteresis + > self.baseline_maximum_hysteresis_rad + ): + failures.append( + { + "joint": joint_name, + "metric": "baseline_hysteresis_deg", + "actual": round( + math.degrees(maximum_baseline_hysteresis), 6 + ), + "limit": round( + math.degrees( + self.baseline_maximum_hysteresis_rad + ), + 6, + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + if fit.circle.get("space") == "image_2d": + checks = ( + ( + "image_radial_rms_px", + float(fit.quality["radial_rms_px"]), + self.image_trajectory_maximum_radial_rms_px, + "maximum", ), - math.degrees( - self.active_maximum_rotation_orthogonal_rms_rad - if profile.joint_specs[joint_name].active - else self.passive_maximum_rotation_orthogonal_rms_rad + ( + "image_radial_p95_px", + float(fit.quality["radial_p95_px"]), + self.image_trajectory_maximum_radial_p95_px, + "maximum", ), - "maximum", - ), - ( - "arc_deg", - math.degrees(float(fit.quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - joint_spec = profile.joint_specs[joint_name] + ( + "image_radius_px", + float(fit.quality["radius_px"]), + self.image_trajectory_minimum_radius_px, + "minimum", + ), + ( + "arc_deg", + math.degrees(float(fit.quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + else: + checks = ( + ( + "rotation_orthogonal_rms_deg", + math.degrees( + float( + fit.quality["rotation_orthogonal_rms_rad"] + ) + ), + math.degrees( + self.active_maximum_rotation_orthogonal_rms_rad + if profile.record_specs[joint_name].active + else self.passive_maximum_rotation_orthogonal_rms_rad + ), + "maximum", + ), + ( + "arc_deg", + math.degrees(float(fit.quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + joint_spec = profile.record_specs[joint_name] monotonic_limit = ( self.maximum_monotonic_correction_rad if joint_spec.active @@ -2304,13 +5747,16 @@ class G20ThreeCameraCalibrationNode(Node): math.degrees(monotonic_limit), "maximum", ), - ( - "hysteresis_deg", - math.degrees(fit.maximum_hysteresis_rad), - math.degrees(hysteresis_limit), - "maximum", - ), ) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + checks = checks + ( + ( + "hysteresis_deg", + math.degrees(fit.maximum_hysteresis_rad), + math.degrees(hysteresis_limit), + "maximum", + ), + ) for metric, actual, limit, comparison in checks: failed = ( actual > limit @@ -2382,13 +5828,24 @@ class G20ThreeCameraCalibrationNode(Node): 1000.0 * axis.radial_rms_m, 1000.0 * self.axis_maximum_radial_rms_m, ), - ( - "axis_pose_line_rms_mm", - 1000.0 * axis.pose_axis_line_rms_m, - 1000.0 * self.axis_maximum_pose_line_rms_m, - ), ] - if joint_name not in zero_profile.constrained_circle_joints: + if ( + joint_spec.zero_kind != "axis_cross_view_validation" + ): + # 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. + axis_checks.append( + ( + "axis_pose_line_rms_mm", + 1000.0 * axis.pose_axis_line_rms_m, + 1000.0 * self.axis_maximum_pose_line_rms_m, + ) + ) + circle_is_constrained = circle_direction_is_constrained( + joint_name, zero_profile.constrained_circle_joints + ) + if not circle_is_constrained: axis_checks.extend( [ ( @@ -2398,7 +5855,7 @@ class G20ThreeCameraCalibrationNode(Node): ), ] ) - if joint_name not in zero_profile.constrained_circle_joints: + if not circle_is_constrained: axis_checks.append( ( "rotation_circle_axis_difference_deg", @@ -2480,6 +5937,86 @@ class G20ThreeCameraCalibrationNode(Node): ) return failures + def _cross_view_roll_diagnostic_role( + self, spec: SweepSpec + ) -> str | None: + finger = str( + getattr(self, "cross_view_roll_diagnostic_finger", "") + ) + if not finger: + return None + if spec.key == f"{finger}_roll_multiview": + return "multiview" + return None + + @staticmethod + def _only_baseline_hysteresis_failures( + failures: Sequence[Mapping[str, Any]], + ) -> bool: + return bool(failures) and all( + str(item.get("metric", "")) + in { + "baseline_hysteresis_deg", + "baseline_directional_gap_deg", + "baseline_directional_gap_range_deg", + } + for item in failures + ) + + def _roll_baseline_hysteresis_degrees( + self, joint_name: str + ) -> list[float]: + records = self.records_by_joint[joint_name] + fit = self._fit_joint_records(joint_name, records) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + spec = profile.record_specs[joint_name] + values = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, joint_name, records + ), + zero_command_u8=int( + self.baseline_command[spec.motor_index] + ), + axis_xyz=fit.circle["axis_xyz"], + ) + return [round(math.degrees(value), 6) for value in values] + + def _complete_cross_view_roll_diagnostic( + self, + side_spec: SweepSpec, + side_quality_failures: Sequence[Mapping[str, Any]] = (), + ) -> None: + finger = self.cross_view_roll_diagnostic_finger + front_joint = f"{finger}_mcp_roll" + side_joint = f"{front_joint}_side" + front_values = self._roll_baseline_hysteresis_degrees(front_joint) + side_values = self._roll_baseline_hysteresis_degrees(side_joint) + limit_deg = math.degrees(self.baseline_maximum_hysteresis_rad) + interpretation = _classify_cross_view_roll_hysteresis( + front_values, side_values, limit_deg=limit_deg + ) + result = { + "kind": "cross_view_roll_diagnostic", + "finger": finger, + "motor_index": side_spec.motor_index, + "front_joint": front_joint, + "side_joint": side_joint, + "front_by_cycle_deg": front_values, + "side_by_cycle_deg": side_values, + "front_maximum_deg": max(front_values), + "side_maximum_deg": max(side_values), + "formal_limit_deg": limit_deg, + "interpretation": interpretation, + "side_quality_failures": [ + dict(item) for item in side_quality_failures + ], + "publication_locked": True, + } + self.cross_view_roll_diagnostic_result = result + self.fit_failure = result + append_jsonl(self.raw_path, result) + self._pause("cross_view_roll_diagnostic_complete") + def _pause_for_provisional_fit_failure( self, spec: SweepSpec, @@ -2487,6 +6024,32 @@ class G20ThreeCameraCalibrationNode(Node): *, allow_warning: bool = False, ) -> bool: + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, spec + ) + ) + if ( + diagnostic_role == "front" + and G20ThreeCameraCalibrationNode._only_baseline_hysteresis_failures( + failures + ) + ): + append_jsonl( + self.raw_path, + { + "kind": "cross_view_roll_front_failure_deferred", + "finger": self.cross_view_roll_diagnostic_finger, + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "failures": failures, + "publication_locked": True, + }, + ) + self.reason = "cross_view_roll_front_failure_deferred" + return False + def is_warning(item: Mapping[str, Any]) -> bool: if "actual" not in item or "limit" not in item: return False @@ -2500,23 +6063,40 @@ class G20ThreeCameraCalibrationNode(Node): return actual <= limit * ratio if allow_warning and failures and all(is_warning(item) for item in failures): + band_attempt = int( + self.sweep_attempts.get(_sweep_storage_key(spec), 1) + ) + retry_limit = int( + getattr(self, "automatic_fit_retry_limit", 0) + ) + # The final fit re-applies the unmodified hard thresholds to the + # same records. Letting even a tiny overrun continue would make + # the final fit recall this task after every later joint has been + # collected. Consume every remaining retry here; when the budget + # is exhausted, the common failure path below pauses in place. append_jsonl( self.raw_path, { - "kind": "provisional_fit_warning", + "kind": ( + "provisional_fit_warning_rescan" + if band_attempt <= retry_limit + else "provisional_fit_warning_retry_exhausted" + ), "view": spec.view, "motor_index": spec.motor_index, "joints": list(spec.joints), + "attempt": band_attempt, + "attempt_limit": retry_limit + 1, "warning_ratio": float( - getattr(self, "provisional_warning_ratio", 1.0) + getattr( + self, "provisional_warning_ratio", 1.0 + ), ), "failures": failures, }, ) - self.reason = "provisional_fit_warning_continuing" - return False - attempt = self.sweep_attempts.get(spec.motor_index, 1) + attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1) self.retry_resume_index = self.sweep_index self.retry_sweep_spec = spec localized_cycles = { @@ -2537,16 +6117,39 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_cycles = localized_cycles else: self.retry_cycles = set(range(self.repetitions)) + history_by_task = getattr( + self, "fit_failure_history_by_task", None + ) + if history_by_task is None: + self.fit_failure_history_by_task = {} + history_by_task = self.fit_failure_history_by_task + task_history = history_by_task.setdefault(spec.key, []) + repeated_branch_clusters = bool( + task_history + and _fit_failure_repeats_branch_clusters( + task_history[-1], failures + ) + ) + task_history.append([dict(item) for item in failures]) + systematic = bool( + _fit_failure_is_systematic(failures, self.repetitions) + or repeated_branch_clusters + ) self.fit_failure = { "kind": "fit_failure", "view": spec.view, + "task_name": spec.key, "motor_index": spec.motor_index, "joints": list(spec.joints), "attempt": attempt, "cycles_to_rescan": [ cycle + 1 for cycle in sorted(self.retry_cycles) ], - "directions_to_rescan": 2 * len(self.retry_cycles), + "directions_to_rescan": ( + 0 if systematic else 2 * len(self.retry_cycles) + ), + "recoverable_by_rescan": not systematic, + "repeated_branch_clusters": repeated_branch_clusters, "failures": failures, } append_jsonl( @@ -2556,6 +6159,13 @@ class G20ThreeCameraCalibrationNode(Node): **self.fit_failure, }, ) + if systematic: + self._pause( + "joint_fit_repeated_branch_failure" + if repeated_branch_clusters + else "joint_fit_systematic_failure" + ) + return True if attempt <= getattr(self, "automatic_fit_retry_limit", 0): self.paused_reason = "joint_fit_check_failed" self.reason = "automatic_retry_joint_fit_check_failed" @@ -2569,6 +6179,20 @@ class G20ThreeCameraCalibrationNode(Node): self, zero_result: ZeroSolveResult ) -> None: """Stop once for a model/zero failure; repeated motion cannot fix it.""" + line_errors_m = dict( + getattr(zero_result, "validation_line_error_by_joint_m", {}) + ) + axis_line_rms_m = float(getattr(zero_result, "axis_line_rms_m", 0.0)) + axis_line_limit_m = float( + getattr(self, "axis_maximum_pose_line_rms_m", 0.0) + ) + observability_rank = int(getattr(zero_result, "observability_rank", 0)) + observability_parameter_count = int( + getattr(zero_result, "observability_parameter_count", 0) + ) + observability_condition_number = float( + getattr(zero_result, "observability_condition_number", 0.0) + ) failures: list[dict[str, Any]] = [] for name, reason in zero_result.failure_reasons.items(): failure: dict[str, Any] = { @@ -2607,6 +6231,21 @@ class G20ThreeCameraCalibrationNode(Node): ) if error > self.maximum_validation_p95_rad ) + failures.extend( + { + "joint": observer, + "metric": "validation_axis_line_residual_mm", + "actual": round(1000.0 * float(error), 6), + "limit": round( + 1000.0 * axis_line_limit_m, 6 + ), + "comparison": "maximum", + } + for observer, error in ( + line_errors_m.items() + ) + if error > axis_line_limit_m + ) direct_joint = next( iter(zero_result.failure_reasons), next( @@ -2633,11 +6272,62 @@ class G20ThreeCameraCalibrationNode(Node): "directions_to_rescan": 0, "recoverable_by_rescan": False, "failures": failures, + "zero_geometry": { + "axis_line_rms_mm": round( + 1000.0 * axis_line_rms_m, 6 + ), + "axis_line_rms_limit_mm": round( + 1000.0 * axis_line_limit_m, 6 + ), + "validation_line_error_by_joint_mm": { + name: round(1000.0 * float(value), 6) + for name, value in ( + line_errors_m.items() + ) + }, + "observability_rank": observability_rank, + "observability_parameter_count": observability_parameter_count, + "observability_condition_number": observability_condition_number, + }, } append_jsonl( self.raw_path, {"kind": "zero_model_failure", **self.fit_failure}, ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + atomic_write_json( + self.session_dir / "g20_right_15_failed_diagnostics.json", + { + "schema_version": 5, + "layout_id": G20_RIGHT_19_LAYOUT, + "passed": False, + "reason": "zero_model_validation_failed", + "failure_reasons": dict(zero_result.failure_reasons), + "axis_line_rms_m": axis_line_rms_m, + "axis_line_rms_limit_m": axis_line_limit_m, + "validation_line_error_by_joint_m": dict( + line_errors_m + ), + "validation_error_by_joint_rad": dict( + zero_result.validation_error_by_joint_rad + ), + "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "offset_confidence_95_half_width_rad": dict( + zero_result.offset_confidence_half_width_rad + ), + "observability": { + "rank": observability_rank, + "parameter_count": observability_parameter_count, + "condition_number": observability_condition_number, + }, + "raw_samples": str(self.raw_path), + "source_urdf": str(self.source_urdf_path), + "camera_extrinsics": str( + self.camera_extrinsics_file + ), + "urdf_published": False, + }, + ) self._pause("zero_model_validation_failed") def _retry_active_sweep_or_pause(self, reason: str) -> None: @@ -2646,7 +6336,9 @@ class G20ThreeCameraCalibrationNode(Node): self._pause(reason) return item = self.active_sweep - key = (item.spec.motor_index, item.cycle, item.direction) + key = ( + _sweep_storage_key(item.spec), item.cycle, item.direction + ) retries = self.sweep_retry_counts.get(key, 0) retry_limit = getattr(self, "automatic_sweep_retry_limit", 0) if retries >= retry_limit: @@ -2654,10 +6346,24 @@ class G20ThreeCameraCalibrationNode(Node): return retries += 1 self.sweep_retry_counts[key] = retries - runtime = getattr(self, "views", {}).get(item.spec.view) - if runtime is not None: + for view in _sweep_views(_node_profile(self), item.spec): + runtime = getattr(self, "views", {}).get(view) + if runtime is None: + continue self._reset_view_trackers(runtime) runtime.pnp_reset_count += 1 + command_records = getattr(self, "command_records_by_joint", {}) + for joint_name in item.spec.joints: + if joint_name in command_records: + command_records[joint_name] = [ + record + for record in command_records[joint_name] + if not ( + str(record.get("task_name")) == item.spec.key + and 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( @@ -2693,50 +6399,331 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = f"automatic_retry_{reason}" self._begin_return_baseline("retry_sweep") + def _record_dedicated_baseline_hold(self, item: SweepItem) -> None: + """Persist one settled baseline observation for one approach path.""" + if not G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( + self, item + ): + return + frames = list(getattr(self, "sweep_baseline_frames", [])) + minimum_frames = int(getattr(self, "minimum_baseline_hold_frames", 10)) + motor = item.spec.motor_index + command = int(self.baseline_command[motor]) + attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) + for joint_name in item.spec.joints: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < minimum_frames: + raise RuntimeError( + "dedicated baseline hold for " + f"{joint_name} has {len(selected)} valid frames; " + f"at least {minimum_frames} required" + ) + state = np.median( + np.asarray( + [frame.state_u8 for frame in selected], dtype=float + ), + axis=0, + ) + quaternion = robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in selected + ] + )[0] + relative_translation = np.median( + np.asarray( + [ + frame.joint_vectors_xyz_m[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + image_relative = np.median( + np.asarray( + [ + frame.image_vectors_xy_px[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + record = { + "kind": "baseline_hold_sample", + "attempt": attempt, + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "command_u8": command, + "relative_quaternion_xyzw": [ + float(value) for value in quaternion + ], + "relative_translation_xyz_m": [ + float(value) for value in relative_translation + ], + "image_relative_xy_px": [ + float(value) for value in image_relative + ], + "parent_pose_common": _robust_pose_payload( + [ + frame.parent_poses_common[joint_name] + for frame in selected + ] + ), + "child_pose_common": _robust_pose_payload( + [ + frame.child_poses_common[joint_name] + for frame in selected + ] + ), + "state_u8": [float(value) for value in state], + "state_image_sync_error_ms": round( + float( + np.percentile( + [ + abs(frame.state_sync_error_ns) + for frame in selected + ], + 95.0, + ) + / 1_000_000.0 + ), + 6, + ), + "pnp_reprojection_error_px": round( + float( + np.percentile( + [ + frame.joint_reprojection_error_px[joint_name] + for frame in selected + ], + 95.0, + ) + ), + 6, + ), + "valid_frames": len(selected), + "hold_seconds": float(self.baseline_hold_seconds), + } + self.baseline_records_by_joint[joint_name].append(record) + if item.cycle == 0: + self.command_records_by_joint[joint_name].append(record) + durable = { + key: value for key, value in record.items() if key != "command_u8" + } + durable["requested_command_u8"] = command + durable["feedback_u8"] = round(float(state[motor]), 6) + durable["image_stamp_ns"] = int( + np.median([frame.stamp_ns for frame in selected]) + ) + append_jsonl(self.raw_path, durable) + + def _baseline_hysteresis_records( + self, + joint_name: str, + trajectory_records: Sequence[Mapping[str, Any]], + ) -> Sequence[Mapping[str, Any]]: + """Use dedicated settled records for non-endpoint baseline joints.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + spec = profile.record_specs[joint_name] + baseline = int(self.baseline_command[spec.motor_index]) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and baseline not in {0, 255} + ): + return self.baseline_records_by_joint.get(joint_name, []) + return trajectory_records + def _finish_active_sweep(self) -> None: assert self.active_sweep is not None item = self.active_sweep motor = item.spec.motor_index - states = np.asarray( - [float(frame.state_u8[motor]) for frame in self.sweep_frames] - ) start_tolerance = self._endpoint_tolerance_for_spec( item.spec, item.start_u8 ) target_tolerance = self._endpoint_tolerance_for_spec( item.spec, item.target_u8 ) - bins: dict[int, list[FrameObservation]] = {} - for frame, state in zip(self.sweep_frames, states): - if abs(state - item.start_u8) <= start_tolerance: - command = item.start_u8 - elif abs(state - item.target_u8) <= target_tolerance: - command = item.target_u8 + joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} + for joint_name in item.spec.joints: + bins: dict[int, list[FrameObservation]] = {} + for frame in _frames_for_joint(self.sweep_frames, joint_name): + state = float(frame.state_u8[motor]) + if abs(state - item.start_u8) <= start_tolerance: + command = item.start_u8 + elif abs(state - item.target_u8) <= target_tolerance: + command = item.target_u8 + else: + command = int(np.clip(np.rint(state), 0, 255)) + bins.setdefault(command, []).append(frame) + commands = sorted(bins) + if not commands or commands[0] != 0 or commands[-1] != 255: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_missing_endpoint_bin", item.spec, joint_name + ) + ) + return + joint_bins[joint_name] = bins + 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. + for joint_name, bins in joint_bins.items(): + commands = sorted(bins) + if min(abs(command - 127) for command in commands) > 2: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "task_precheck_missing_command_127", + item.spec, + joint_name, + ) + ) + return + detection_rates: dict[str, float] = {} + total_by_view = getattr( + self, "sweep_detection_total_by_view", {} + ) + valid_by_view = getattr( + self, "sweep_detection_valid_by_view", {} + ) + capture_views = _sweep_views(_node_profile(self), item.spec) + for view in capture_views: + if view in total_by_view: + total = int(total_by_view.get(view, 0)) + valid = int(valid_by_view.get(view, 0)) + elif len(capture_views) == 1: + # Compatibility for legacy sessions and focused unit + # harnesses created before per-camera counters existed. + total = int( + getattr(self, "sweep_detection_total_frames", 0) + ) + valid = int( + getattr(self, "sweep_detection_valid_frames", 0) + ) + else: + total = 0 + valid = 0 + 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 + minimum_bin_count = min( + len(bins) for bins in joint_bins.values() + ) + precheck_maximum_bin_gap = max( + int(max(np.diff(sorted(bins)), default=0)) + for bins in joint_bins.values() + ) + minimum_valid_frames = min( + sum(len(frames) for frames in bins.values()) + for bins in joint_bins.values() + ) + G20ThreeCameraCalibrationNode._record_precheck_speed_metric( + self, + item, + bin_count=minimum_bin_count, + maximum_bin_gap=precheck_maximum_bin_gap, + valid_frames=minimum_valid_frames, + ) + append_jsonl( + self.raw_path, + { + "kind": "task_visibility_precheck", + "task_name": item.spec.key, + "view": item.spec.view, + "capture_views": list( + _sweep_views(_node_profile(self), item.spec) + ), + "motor_index": motor, + "direction": item.direction, + "checked_commands_u8": [0, 127, 255], + "bin_count": minimum_bin_count, + "maximum_bin_gap": precheck_maximum_bin_gap, + "detection_rate": round(min(detection_rates.values()), 6), + "detection_rate_by_view": { + view: round(rate, 6) + for view, rate in detection_rates.items() + }, + "passed": True, + }, + ) + if not getattr(self, "active_sweep_is_fit_retry", False): + self.sweep_index += 1 + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps( + self + ) + self.active_sweep = None + self.active_sweep_is_fit_retry = False + 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 + ): + self._begin_return_baseline("next_cycle") + else: + self._start_next_sweep() else: - command = int(np.clip(np.rint(state), 0, 255)) - bins.setdefault(command, []).append(frame) - commands = sorted(bins) - if len(commands) < self.minimum_sweep_bins: - self._retry_active_sweep_or_pause("sweep_bins_too_few") - return - if commands[0] != 0 or commands[-1] != 255: - self._retry_active_sweep_or_pause("sweep_missing_endpoint_bin") - return - if max(np.diff(commands), default=0) > self.maximum_bin_gap: - self._retry_active_sweep_or_pause("sweep_bin_gap_too_large") + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, item.spec + ) + ) return - append_jsonl_many( - self.raw_path, - ( - { + for joint_name, bins in joint_bins.items(): + commands = sorted(bins) + if len(commands) < self.minimum_sweep_bins: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_bins_too_few", item.spec, joint_name + ) + ) + return + if max(np.diff(commands), default=0) > self.maximum_bin_gap: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_bin_gap_too_large", item.spec, joint_name + ) + ) + return + + def synchronised_frame_payload( + frame: FrameObservation, + ) -> dict[str, Any]: + names = [ + name + for name in item.spec.joints + if name in frame.joint_quaternions_xyzw + ] + return { "kind": "synchronised_frame", - "attempt": self.sweep_attempts.get(motor, 1), - "view": item.spec.view, + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": frame.view, "motor_index": motor, - "joints": list(item.spec.joints), + "joints": names, "cycle": item.cycle, "direction": item.direction, + "requested_command_u8": int( + self.sweep_checkpoint_target_u8 + if self.sweep_checkpoint_target_u8 is not None + else item.target_u8 + ), + "feedback_u8": round(float(frame.state_u8[motor]), 6), "image_stamp_ns": int(frame.stamp_ns), "actual_state_u8": [ float(value) for value in frame.state_u8 @@ -2746,26 +6733,36 @@ class G20ThreeCameraCalibrationNode(Node): ), "relative_quaternion_xyzw": { name: list(frame.joint_quaternions_xyzw[name]) - for name in item.spec.joints + for name in names }, "parent_pose_common": { name: frame.parent_poses_common[name] - for name in item.spec.joints + for name in names }, "child_pose_common": { name: frame.child_poses_common[name] - for name in item.spec.joints + for name in names }, "pnp_reprojection_error_px": { name: frame.joint_reprojection_error_px[name] - for name in item.spec.joints + for name in names }, } + append_jsonl_many( + self.raw_path, + ( + synchronised_frame_payload(frame) for frame in self.sweep_frames + if any( + name in frame.joint_quaternions_xyzw + for name in item.spec.joints + ) ), ) for joint_name in item.spec.joints: + bins = joint_bins[joint_name] + commands = sorted(bins) for command in commands: frames = bins[command] vector = np.median( @@ -2804,8 +6801,11 @@ class G20ThreeCameraCalibrationNode(Node): ) record = { "kind": "sample", - "attempt": self.sweep_attempts.get(motor, 1), - "view": item.spec.view, + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, "joint": joint_name, "motor_index": motor, "cycle": item.cycle, @@ -2851,21 +6851,54 @@ class G20ThreeCameraCalibrationNode(Node): "valid_frames": len(frames), } self.records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, record) + durable = { + key: value for key, value in record.items() if key != "command_u8" + } + durable["requested_command_u8"] = int( + self.sweep_checkpoint_target_u8 + if self.sweep_checkpoint_target_u8 is not None + else item.target_u8 + ) + durable["feedback_u8"] = int(command) + durable["image_stamp_ns"] = int( + np.median([frame.stamp_ns for frame in frames]) + ) + append_jsonl(self.raw_path, durable) + G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold( + self, item + ) previous_spec = item.spec was_fit_retry = bool( getattr(self, "active_sweep_is_fit_retry", False) ) if not was_fit_retry: self.sweep_index += 1 + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) self.active_sweep = None self.active_sweep_is_fit_retry = False if was_fit_retry: if self.retry_sweep_items: - self._start_next_sweep() + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and item.direction == DIRECTION_INCREASING + and self.retry_sweep_items[0].spec == previous_spec + ): + self._begin_return_baseline("next_cycle") + else: + self._start_next_sweep() return failures = self._provisional_fit_failures(previous_spec) + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, previous_spec + ) + ) + if diagnostic_role in {"side", "multiview"}: + G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( + self, previous_spec, failures + ) + return if failures and self._pause_for_provisional_fit_failure( previous_spec, failures, allow_warning=True ): @@ -2876,7 +6909,11 @@ class G20ThreeCameraCalibrationNode(Node): if self.sweep_index >= len(self.sweep_items): self._begin_return_baseline("fit") else: - self._begin_return_baseline("next_sweep") + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, previous_spec + ) + ) return spec_complete = bool( self.sweep_index >= len(self.sweep_items) @@ -2884,6 +6921,16 @@ class G20ThreeCameraCalibrationNode(Node): ) if spec_complete: failures = self._provisional_fit_failures(previous_spec) + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, previous_spec + ) + ) + if diagnostic_role in {"side", "multiview"}: + G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( + self, previous_spec, failures + ) + return if failures and self._pause_for_provisional_fit_failure( previous_spec, failures, allow_warning=True ): @@ -2898,7 +6945,20 @@ class G20ThreeCameraCalibrationNode(Node): if self.sweep_index >= len(self.sweep_items): self._begin_return_baseline("fit") elif self.sweep_items[self.sweep_index].spec != previous_spec: - self._begin_return_baseline("next_sweep") + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, previous_spec + ) + ) + elif ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and item.direction == DIRECTION_INCREASING + ): + # Keep the task's complete avoidance pose between its three + # rounds. 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: self._start_next_sweep() @@ -2917,17 +6977,25 @@ class G20ThreeCameraCalibrationNode(Node): training_fits: dict[str, JointCurveFit] = {} holdout_by_joint: dict[str, tuple[float, ...]] = {} measured: dict[str, JointCurveFit] = {} + validation_cycle = self.repetitions - 1 + training_cycles = tuple(range(validation_cycle)) + training_cycle_set = set(training_cycles) for name in self.profile.measured_joints: training_records = [ record for record in self.records_by_joint[name] - if int(record["cycle"]) in {0, 1} + if int(record["cycle"]) in training_cycle_set ] holdout_records = [ record for record in self.records_by_joint[name] - if int(record["cycle"]) == 2 + if int(record["cycle"]) == validation_cycle ] + holdout_model_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, name, holdout_records + ) + ) zero_command = int( self.baseline_command[ self.profile.joint_specs[name].motor_index @@ -2936,14 +7004,226 @@ class G20ThreeCameraCalibrationNode(Node): training_fits[name] = self._fit_joint_records( name, training_records ) - holdout_by_joint[name] = rotation_curve_holdout_errors( + holdout_by_joint[name] = joint_curve_holdout_errors( training_fits[name], - holdout_records, + holdout_model_records, zero_command_u8=zero_command, ) measured[name] = self._fit_joint_records( name, self.records_by_joint[name] ) + command_fits: dict[str, JointCurveFit] = dict(training_fits) + command_feedback_fits: dict[str, JointCurveFit] = dict( + training_fits + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + command_fits = {} + command_feedback_fits = {} + for name in self.profile.measured_joints: + if name in G20_REFERENCE_THUMB_CMC_JOINTS: + # a609d521's dense feedback-indexed relative-rotation + # curve is the validated transfer function for the three + # CMC axes. Do not replace it with nine planar PnP + # checkpoints; those points were the source of the later + # thumb regression and combination-pose false failures. + command_fits[name] = measured[name] + continue + command_records = [ + dict(record) + for record in self.command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + if len(command_records) < 18: + failed_spec = next( + spec + for spec in self.profile.sweep_specs + if name in spec.joints + ) + self._pause_for_provisional_fit_failure( + failed_spec, + [ + { + "joint": name, + "metric": "steady_command_checkpoints", + "actual": len(command_records), + "limit": 18, + "comparison": "minimum", + } + ], + ) + return + zero_command = int( + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] + ) + if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS: + command_fits[name] = fit_joint_image_curve( + command_records, + maximum_radial_rms_px=( + self.image_trajectory_maximum_radial_rms_px + ), + maximum_radial_p95_px=( + self.image_trajectory_maximum_radial_p95_px + ), + minimum_radius_px=( + self.image_trajectory_minimum_radius_px + ), + minimum_arc_rad=self.trajectory_minimum_arc_rad, + ) + else: + command_fits[name] = fit_rotation_joint_curve( + command_records, + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + self.profile, name + ), + ) + command_feedback_fits[name] = self._fit_joint_records( + name, + _steady_records_in_feedback_domain(command_records), + ) + self.cross_view_roll_metrics: dict[str, dict[str, float]] = {} + self.validation_only_fits: dict[str, JointCurveFit] = {} + for name, validation_name in ( + self.profile.axis_validation_sources or {} + ).items(): + validation_training_records = [ + record + for record in self.records_by_joint[validation_name] + if int(record["cycle"]) in training_cycle_set + ] + validation_holdout_records = [ + record + for record in self.records_by_joint[validation_name] + if int(record["cycle"]) == validation_cycle + ] + validation_holdout_model_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + validation_name, + validation_holdout_records, + ) + ) + validation_training_fit = self._fit_joint_records( + validation_name, validation_training_records + ) + validation_fit = self._fit_joint_records( + validation_name, self.records_by_joint[validation_name] + ) + # Any optional post-fit random validation must query the same + # frozen training model that passed the isolated holdout. + 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 + validation_errors = joint_curve_holdout_errors( + validation_training_fit, + validation_holdout_model_records, + zero_command_u8=int( + self.baseline_command[ + self.profile.record_specs[validation_name].motor_index + ] + ), + ) + holdout_by_joint[validation_name] = validation_errors + self.cross_view_roll_metrics[name] = { + **{ + f"training_{key}": float(value) + for key, value in training_metrics.items() + }, + **{ + f"final_{key}": float(value) + for key, value in final_metrics.items() + }, + } + self.joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} + for name in self.profile.measured_joints: + cycle_travel: list[float] = [] + for cycle in range(self.repetitions): + cycle_fit = self._fit_joint_records( + name, + [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) == cycle + ], + ) + cycle_travel.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + holdout = np.abs( + np.asarray(holdout_by_joint[name], dtype=float) + ) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, name, self.records_by_joint[name] + ), + zero_command_u8=int( + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] + ), + axis_xyz=( + G20ThreeCameraCalibrationNode._hysteresis_axis_for_fit( + self, + name, + self.records_by_joint[name], + measured[name], + ) + ), + ) + self.joint_dynamic_diagnostics[name] = { + "cycle_travel_rad": [ + round(float(value), 8) for value in cycle_travel + ], + "cycle_travel_range_rad": round( + max(cycle_travel) - min(cycle_travel), 8 + ), + "baseline_hysteresis_by_cycle_rad": [ + round(float(value), 8) + for value in baseline_hysteresis + ], + "holdout_cycle": validation_cycle, + "holdout_cycle_mae_rad": round(float(np.mean(holdout)), 8), + "holdout_cycle_max_rad": round(float(np.max(holdout)), 8), + } axes: list[JointAxisMeasurement] = [] for name in self.zero_profile.axis_joints: for cycle in range(self.repetitions): @@ -2962,9 +7242,22 @@ class G20ThreeCameraCalibrationNode(Node): joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, maximum_validation_mae_rad=self.maximum_validation_mae_rad, maximum_validation_p95_rad=self.maximum_validation_p95_rad, + maximum_validation_error_rad=( + self.maximum_validation_error_rad + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + maximum_confidence_half_width_rad=( + self.zero_maximum_confidence_half_width_rad + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), maximum_axis_cone_mismatch_rad=( self.zero_maximum_axis_cone_mismatch_rad ), + maximum_observability_condition_number=( + self.zero_maximum_observability_condition_number + ), maximum_pose_axis_line_rms_m=( self.axis_maximum_pose_line_rms_m ), @@ -2973,6 +7266,9 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cycle_difference_rad ), hand_type=self.hand_type, + tag_layout=self.profile.layout_id, + training_cycles=training_cycles, + validation_cycle=validation_cycle, ) append_jsonl( self.raw_path, @@ -2983,6 +7279,12 @@ class G20ThreeCameraCalibrationNode(Node): "direct_offsets_rad": dict( holdout_zero_result.direct_offsets_rad ), + "model_base_translation_xyz_m": list( + holdout_zero_result.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + holdout_zero_result.base_quaternion_xyzw + ), "cycle_offsets_rad": { name: list(values) for name, values in ( @@ -3005,6 +7307,20 @@ class G20ThreeCameraCalibrationNode(Node): holdout_zero_result .validation_improvement_confidence_lower_rad ), + "axis_line_rms_m": holdout_zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": dict( + holdout_zero_result.validation_line_error_by_joint_m + ), + "offset_confidence_95_half_width_rad": dict( + holdout_zero_result.offset_confidence_half_width_rad + ), + "observability_rank": holdout_zero_result.observability_rank, + "observability_parameter_count": ( + holdout_zero_result.observability_parameter_count + ), + "observability_condition_number": ( + holdout_zero_result.observability_condition_number + ), "failure_reasons": dict( holdout_zero_result.failure_reasons ), @@ -3026,6 +7342,21 @@ class G20ThreeCameraCalibrationNode(Node): <= self.maximum_validation_mae_rad and float(np.percentile(holdout_errors, 95.0)) <= self.maximum_validation_p95_rad + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or float(np.max(holdout_errors)) + <= self.maximum_validation_error_rad + ) + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or all( + float(np.mean(np.abs(np.asarray(values, dtype=float)))) + <= self.maximum_validation_mae_rad + and float(np.max(np.abs(np.asarray(values, dtype=float)))) + <= self.maximum_validation_error_rad + for values in holdout_by_joint.values() + ) + ) ) self.fit_quality_passed = all( fit.maximum_monotonic_correction_rad @@ -3034,13 +7365,20 @@ class G20ThreeCameraCalibrationNode(Node): if self.profile.joint_specs[name].active else self.passive_maximum_monotonic_correction_rad ) - and fit.maximum_hysteresis_rad - <= ( - self.maximum_hysteresis_rad - if self.profile.joint_specs[name].active - else self.passive_maximum_hysteresis_rad + and ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + or fit.maximum_hysteresis_rad + <= ( + self.maximum_hysteresis_rad + if self.profile.joint_specs[name].active + else self.passive_maximum_hysteresis_rad + ) ) - for name, fit in measured.items() + for name, fit in training_fits.items() + ) and all( + fit.maximum_hysteresis_rad + <= self.command_maximum_direction_gap_rad + for fit in command_feedback_fits.values() ) if not trajectory_holdout_passed: trajectory_score = { @@ -3066,46 +7404,95 @@ class G20ThreeCameraCalibrationNode(Node): for name, error in trajectory_score.items() if error > self.maximum_validation_p95_rad ] + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + for name, values in holdout_by_joint.items(): + absolute = np.abs(np.asarray(values, dtype=float)) + mean_error = float(np.mean(absolute)) + maximum_error = float(np.max(absolute)) + if mean_error > self.maximum_validation_mae_rad: + failures.append( + { + "joint": name, + "metric": "third_cycle_trajectory_mae_deg", + "actual": round( + math.degrees(mean_error), 6 + ), + "limit": math.degrees( + self.maximum_validation_mae_rad + ), + "comparison": "maximum", + } + ) + if maximum_error > self.maximum_validation_error_rad: + failures.append( + { + "joint": name, + "metric": "third_cycle_trajectory_max_deg", + "actual": round( + math.degrees(maximum_error), 6 + ), + "limit": math.degrees( + self.maximum_validation_error_rad + ), + "comparison": "maximum", + } + ) self._pause_for_provisional_fit_failure(failed_spec, failures) return if not holdout_zero_result.passed: self._pause_for_zero_model_failure(holdout_zero_result) return - zero_result = solve_urdf_zero_offsets( - source_urdf=self.source_urdf_path, - measurements=axes, - curves=measured, - motor_by_joint=motor_by_joint, - maximum_offset_rad=self.zero_maximum_offset_rad, - joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, - maximum_validation_mae_rad=self.maximum_validation_mae_rad, - maximum_validation_p95_rad=self.maximum_validation_p95_rad, - maximum_axis_cone_mismatch_rad=( - self.zero_maximum_axis_cone_mismatch_rad - ), - maximum_pose_axis_line_rms_m=( - self.axis_maximum_pose_line_rms_m - ), - finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, - maximum_cycle_difference_rad=( - self.zero_maximum_axis_cycle_difference_rad - ), - hand_type=self.hand_type, - ) - if not zero_result.passed: - raise RuntimeError("final_all_cycle_zero_refit_failed") + # Publish the exact frozen training model that was evaluated on the + # isolated final cycle. Re-solving with the holdout would invalidate + # the acceptance result. + zero_result = holdout_zero_result append_jsonl( self.raw_path, { "kind": "zero_final_diagnostics", "hand_type": self.hand_type, "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "model_base_translation_xyz_m": list( + zero_result.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + zero_result.base_quaternion_xyzw + ), "offset_uncertainty_rad": dict( zero_result.offset_uncertainty_rad ), + "offset_confidence_95_half_width_rad": dict( + zero_result.offset_confidence_half_width_rad + ), + "training_cycles": list(zero_result.training_cycles), + "validation_cycle": zero_result.validation_cycle, + "axis_line_rms_m": zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": dict( + zero_result.validation_line_error_by_joint_m + ), + "observability_rank": zero_result.observability_rank, + "observability_parameter_count": ( + zero_result.observability_parameter_count + ), + "observability_condition_number": ( + zero_result.observability_condition_number + ), + "offset_covariance_rad2": dict( + zero_result.offset_covariance_rad2 + ), }, ) - self.measured_fits = measured + # Runtime/MuJoCo consumes requested commands, never feedback bins. + # Static geometry and holdout above intentionally continue to use the + # dense feedback-domain training fit. + self.measured_fits = clamp_runtime_fits_to_urdf_limits( + self.source_urdf_path, + derive_mimic_passive_fits( + self.source_urdf_path, + command_fits, + profile=self.profile, + ), + ) self.axis_measurements = axes self.zero_result = zero_result self.validation_errors_rad = [ @@ -3117,7 +7504,10 @@ class G20ThreeCameraCalibrationNode(Node): float(value) for value in holdout_zero_result.validation_errors_rad ) - if self.validation_enabled: + if self.combination_validation_enabled: + self._build_combination_validation_items() + self._start_next_combination_validation() + elif self.validation_enabled: self._build_validation_items() self._start_next_validation() else: @@ -3137,12 +7527,386 @@ class G20ThreeCameraCalibrationNode(Node): ) self.validation_index = 0 + def _build_combination_validation_items(self) -> None: + self.combination_validation_items = list( + _combination_validation_items(self.baseline_command) + ) + self.combination_validation_index = 0 + self.combination_validation_completed = False + + def _start_next_combination_validation(self) -> None: + if self.combination_validation_index >= len( + self.combination_validation_items + ): + self.active_combination_validation = None + self.combination_validation_completed = True + self._begin_return_baseline("finalize") + return + item = self.combination_validation_items[ + self.combination_validation_index + ] + self.active_combination_validation = item + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.position_hold_since = None + self.validation_stage_started_at = time.monotonic() + self.motion_stage_started_at = self.validation_stage_started_at + self._reset_motion_progress( + self.validation_stage_started_at, + self._command_vector_error_u8(item.command_u8), + ) + self.state = STATE_VALIDATION_MOVE + self.reason = f"combination_validation_move_{item.name}" + self._publish_speed_profile(self._normal_speed_profile()) + self._publish_command(list(item.command_u8)) + + def _finish_combination_validation_capture(self) -> None: + item = self.active_combination_validation + if item is None: + raise RuntimeError("combination validation item is missing") + observations: dict[str, Any] = {} + all_frames = [ + frame + for frames in self.combination_validation_frames_buffer.values() + for frame in frames + ] + feedback = np.median( + np.asarray([frame.state_u8 for frame in all_frames], dtype=float), + axis=0, + ) + motor_directions = _combination_motor_directions( + self.combination_validation_items, + self.combination_validation_index, + self.baseline_command, + ) + angles = _combination_joint_angles( + self.profile, + self.measured_fits, + item.command_u8, + motor_directions, + ) + model = UrdfKinematicModel(self.source_urdf_path) + model_base_common = transform_matrix( + self.zero_result.base_translation_xyz_m, + self.zero_result.base_quaternion_xyzw, + ) + pose_position_errors: list[float] = [] + pose_orientation_errors: list[float] = [] + target_errors: list[dict[str, Any]] = [] + observed_target_keys: list[str] = [] + evaluated_target_keys: list[str] = [] + pending_mounts: dict[str, np.ndarray] = {} + for view, frames in self.combination_validation_frames_buffer.items(): + by_joint: dict[str, Any] = {} + joint_names = sorted( + set.intersection( + *(set(frame.joint_quaternions_xyzw) for frame in frames) + ) + ) + for name in joint_names: + by_joint[name] = { + "relative_quaternion_xyzw": [ + float(value) + for value in robust_rotation_summary( + [frame.joint_quaternions_xyzw[name] for frame in frames] + )[0] + ], + "parent_pose_common": _robust_pose_payload( + [frame.parent_poses_common[name] for frame in frames] + ), + "child_pose_common": _robust_pose_payload( + [frame.child_poses_common[name] for frame in frames] + ), + } + observations[view] = by_joint + base_role = COMBINATION_BASE_ROLE_BY_VIEW[view] + base_candidates = [ + name + for name in joint_names + if self.profile.record_specs[name].parent_role == base_role + ] + preferred_base = COMBINATION_BASE_OBSERVER_BY_VIEW[view] + if preferred_base in base_candidates: + base_name = preferred_base + elif base_candidates: + base_name = base_candidates[0] + else: + self._retry_combination_validation_or_pause( + f"combination_{view}_has_no_visible_base_target", + time.monotonic(), + ) + return + base_pose = _robust_pose_payload( + [frame.parent_poses_common[base_name] for frame in frames] + ) + base_matrix = transform_matrix( + base_pose["translation_xyz_m"], base_pose["quaternion_xyzw"] + ) + for observation_name, model_joint in COMBINATION_TAG_TARGETS_BY_VIEW[view]: + if observation_name not in by_joint: + continue + child_pose = by_joint[observation_name]["child_pose_common"] + child_matrix = transform_matrix( + child_pose["translation_xyz_m"], + child_pose["quaternion_xyzw"], + ) + observed = np.linalg.inv(base_matrix) @ child_matrix + # ``observed`` is expressed in this view's fixed palm-Tag + # frame, while UrdfKinematicModel returns hand_base->link. + # The zero solve already estimates hand_base->common; carry + # the model through common and into the observer base frame + # before solving or applying the moving Tag mount. + link = _model_link_in_observer_base( + base_matrix, + model_base_common, + model.link_transform( + model_joint, + zero_offsets=( + self.zero_result.all_active_offsets_rad + ), + joint_angles=angles, + independent_mimic_angles=True, + ), + ) + key = f"{view}:{observation_name}" + quality_gated = key in G20_COMBINATION_REQUIRED_TARGET_KEYS + observed_target_keys.append(key) + if key not in self.combination_tag_mounts: + pending_mounts[key] = np.linalg.inv(link) @ observed + continue + else: + mount = self.combination_tag_mounts[key] + predicted = link @ mount + if quality_gated: + evaluated_target_keys.append(key) + position_error = float( + np.linalg.norm(predicted[:3, 3] - observed[:3, 3]) + ) + orientation_error = float( + ( + Rotation.from_matrix(predicted[:3, :3]).inv() + * Rotation.from_matrix(observed[:3, :3]) + ).magnitude() + ) + if quality_gated: + pose_position_errors.append(position_error) + pose_orientation_errors.append(orientation_error) + target_errors.append( + { + "target": key, + "model_joint": model_joint, + "quality_gated": quality_gated, + "position_error_m": round(position_error, 8), + "orientation_error_rad": round( + orientation_error, 8 + ), + "orientation_error_deg": round( + math.degrees(orientation_error), 6 + ), + "observed_pose_in_base": matrix_payload(observed), + "predicted_pose_in_base": matrix_payload(predicted), + } + ) + if self.combination_validation_index > 0 and not pose_position_errors: + self._retry_combination_validation_or_pause( + "combination_pose_has_no_previously_observed_visible_target", + time.monotonic(), + ) + return + position_p95 = ( + 0.0 + if not pose_position_errors + else float(np.percentile(pose_position_errors, 95.0)) + ) + orientation_p95 = ( + 0.0 + if not pose_orientation_errors + else float(np.percentile(pose_orientation_errors, 95.0)) + ) + if ( + position_p95 > self.combination_maximum_position_p95_m + or orientation_p95 > self.combination_maximum_orientation_p95_rad + ): + retry_key = ("combination", self.combination_validation_index) + append_jsonl( + self.raw_path, + { + "kind": "combination_validation_failure", + "reason": "combination_pose_prediction_failed", + "pose_index": self.combination_validation_index, + "pose_name": item.name, + "label_zh": item.label_zh, + "capture_attempt": ( + self.validation_retry_counts.get(retry_key, 0) + 1 + ), + "requested_command_u8": list(item.command_u8), + "feedback_u8": [ + round(float(value), 6) for value in feedback + ], + "angle_branch_by_motor": list(motor_directions), + "evaluated_targets": sorted(set(evaluated_target_keys)), + "target_errors": sorted( + target_errors, key=lambda value: value["target"] + ), + "valid_frames_by_view": { + view: len(frames) + for view, frames in ( + self.combination_validation_frames_buffer.items() + ) + }, + "position_p95_m": round(position_p95, 8), + "maximum_position_p95_m": round( + self.combination_maximum_position_p95_m, 8 + ), + "orientation_p95_rad": round(orientation_p95, 8), + "orientation_p95_deg": round( + math.degrees(orientation_p95), 6 + ), + "maximum_orientation_p95_rad": round( + self.combination_maximum_orientation_p95_rad, 8 + ), + "maximum_orientation_p95_deg": round( + math.degrees( + self.combination_maximum_orientation_p95_rad + ), + 6, + ), + "passed": False, + }, + ) + self._retry_combination_validation_or_pause( + "combination_pose_prediction_failed", time.monotonic() + ) + return + self.combination_tag_mounts.update(pending_mounts) + for key in set(observed_target_keys): + self.combination_tag_observation_counts[key] = ( + self.combination_tag_observation_counts.get(key, 0) + 1 + ) + for key in set(evaluated_target_keys): + self.combination_tag_validation_counts[key] = ( + self.combination_tag_validation_counts.get(key, 0) + 1 + ) + is_final_pose = self.combination_validation_index + 1 >= len( + self.combination_validation_items + ) + if is_final_pose: + coverage = combination_target_coverage( + self.combination_tag_observation_counts, + self.combination_tag_validation_counts, + ) + if not coverage["coverage_passed"]: + append_jsonl( + self.raw_path, + { + "kind": "combination_target_coverage_failure", + "pose_name": item.name, + **coverage, + }, + ) + self._retry_combination_validation_or_pause( + "combination_target_coverage_incomplete", time.monotonic() + ) + return + self.combination_position_errors_m.extend(pose_position_errors) + self.combination_orientation_errors_rad.extend(pose_orientation_errors) + append_jsonl( + self.raw_path, + { + "kind": "combination_validation_sample", + "pose_index": self.combination_validation_index, + "pose_name": item.name, + "label_zh": item.label_zh, + "requested_command_u8": list(item.command_u8), + "feedback_u8": [round(float(value), 6) for value in feedback], + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in all_frames]) + ), + "views": observations, + "observed_targets": sorted(set(observed_target_keys)), + "evaluated_targets": sorted(set(evaluated_target_keys)), + "valid_frames_by_view": { + view: len(frames) + for view, frames in self.combination_validation_frames_buffer.items() + }, + "position_p95_m": round(position_p95, 8), + "orientation_p95_rad": round(orientation_p95, 8), + "passed": True, + }, + ) + self.combination_validation_index += 1 + self.active_combination_validation = None + if self.combination_validation_index >= len( + self.combination_validation_items + ): + self.combination_validation_completed = True + self._begin_return_baseline("finalize") + else: + self._begin_return_baseline("combination_next") + + def _retry_combination_validation_or_pause( + self, reason: str, now: float + ) -> None: + item = self.active_combination_validation + if item is None: + self._pause(reason) + return + key = ("combination", self.combination_validation_index) + retries = self.validation_retry_counts.get(key, 0) + if retries >= self.automatic_sweep_retry_limit: + self._pause(reason) + return + retries += 1 + self.validation_retry_counts[key] = retries + for view in self.combination_validation_frames_buffer: + runtime = self.views.get(view) + if runtime is None: + continue + self._reset_view_trackers(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + append_jsonl( + self.raw_path, + { + "kind": "automatic_combination_retry", + "pose_name": item.name, + "reason": reason, + "retry": retries, + "retry_limit": self.automatic_sweep_retry_limit, + }, + ) + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.validation_stage_started_at = now + self.motion_stage_started_at = now + self.position_hold_since = None + self.state = STATE_VALIDATION_MOVE + self.reason = f"automatic_retry_{reason}" + self._reset_motion_progress(now, self._command_vector_error_u8(item.command_u8)) + self._publish_command(list(item.command_u8)) + def _start_next_validation(self) -> None: if self.validation_index >= len(self.validation_items): self.active_validation = None self._begin_return_baseline("finalize") return self.active_validation = self.validation_items[self.validation_index] + validation_motor = self.active_validation.spec.motor_index + validation_target = float(self.active_validation.command_u8) + validation_current = ( + float(self.latest_state_u8[validation_motor]) + if len(self.latest_state_u8) == 20 + else validation_target + ) + self.active_validation_direction = ( + DIRECTION_INCREASING + if validation_target > validation_current + 0.5 + else DIRECTION_DECREASING + if validation_target < validation_current - 0.5 + else canonical_zero_direction( + self.profile, self.active_validation.spec.joints[0] + ) + ) self.validation_frames_buffer.clear() self.position_hold_since = None self.validation_stage_started_at = time.monotonic() @@ -3176,21 +7940,50 @@ class G20ThreeCameraCalibrationNode(Node): item = self.active_validation command = item.command_u8 for name in item.spec.joints: + selected = _frames_for_joint( + self.validation_frames_buffer, name + ) + if len(selected) < self.validation_frames: + raise RuntimeError( + f"random validation {name} has too few frames" + ) + fit = self.measured_fits.get(name) + if fit is None: + fit = self.validation_only_fits[name] quaternion = robust_rotation_summary( [ frame.joint_quaternions_xyzw[name] - for frame in self.validation_frames_buffer + for frame in selected ] )[0] - observed = measure_rotation_joint_observation( - self.measured_fits[name], - quaternion, + image_relative_xy_px = np.median( + np.asarray( + [ + frame.image_vectors_xy_px[name] + for frame in selected + ], + dtype=float, + ), + axis=0, ) - expected = float(self.measured_fits[name].angle_rad[command]) + observed = measure_joint_curve_observation( + fit, + quaternion_xyzw=quaternion, + image_relative_xy_px=image_relative_xy_px, + ) + expected_curve = ( + fit.decreasing_rad + if self.active_validation_direction == DIRECTION_DECREASING + else fit.increasing_rad + if self.active_validation_direction == DIRECTION_INCREASING + else fit.angle_rad + ) + expected = float(expected_curve[command]) self.validation_errors_rad.append(observed - expected) previous_spec = item.spec self.validation_index += 1 self.active_validation = None + self.active_validation_direction = None if self.validation_index >= len(self.validation_items): self._begin_return_baseline("finalize") elif self.validation_items[self.validation_index].spec != previous_spec: @@ -3204,6 +7997,10 @@ class G20ThreeCameraCalibrationNode(Node): errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) validation_passed = bool( self.zero_result.passed + and ( + not self.combination_validation_enabled + or self.combination_validation_completed + ) and ( not self.validation_enabled or ( @@ -3212,6 +8009,11 @@ class G20ThreeCameraCalibrationNode(Node): <= self.maximum_validation_mae_rad and float(np.percentile(errors, 95.0)) <= self.maximum_validation_p95_rad + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or float(np.max(errors)) + <= self.maximum_validation_error_rad + ) ) ) ) @@ -3226,23 +8028,99 @@ class G20ThreeCameraCalibrationNode(Node): # Zero calibration changes only joint-origin rotations. Measured # motion curves belong in schema-v4 JSON and must never enlarge the # original CAD/mechanical safety limits in the generated URDF. + # JSON schema v4 publishes eight decimal places. Generate the URDF + # from those exact same values so the pair is numerically inseparable + # instead of differing by harmless formatter quantisation. + published_zero_offsets = { + name: round(float(value), 8) + for name, value in self.zero_result.all_active_offsets_rad.items() + } + urdf_offsets = published_zero_offsets + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + urdf_offsets = { + name: published_zero_offsets[name] + for name in self.zero_profile.direct_zero_joints + } self.corrected_urdf_path = write_zero_corrected_urdf( source_urdf=self.source_urdf_path, output_directory=self.corrected_urdf_output_dir, serial_number=self.serial_number, - offsets_rad=self.zero_result.all_active_offsets_rad, + offsets_rad=urdf_offsets, timestamp=stamp, ) - payload = build_compact_payload( - serial_number=self.serial_number, - measured_fits=self.measured_fits, - urdf_zero_offsets_rad=self.zero_result.all_active_offsets_rad, - validation_errors_rad=self.validation_errors_rad, - passed=passed, - baseline=self.baseline_command, - side=self.hand_type, - ) - atomic_write_json(self.final_path, payload) + try: + payload = build_compact_payload( + serial_number=self.serial_number, + measured_fits=self.measured_fits, + urdf_zero_offsets_rad=published_zero_offsets, + validation_errors_rad=self.validation_errors_rad, + passed=passed, + baseline=self.baseline_command, + side=self.hand_type, + layout_id=self.profile.layout_id, + zero_uncertainty_rad=( + self.zero_result.offset_confidence_half_width_rad + ), + zero_cycle_offsets_rad=self.zero_result.cycle_offsets_rad, + zero_observers=self.zero_profile.offset_observer_joint, + artifact_hashes=( + { + "source_urdf_sha256": _file_sha256( + self.source_urdf_path + ), + "camera_extrinsics_sha256": _file_sha256( + self.camera_extrinsics_file + ), + "corrected_urdf_sha256": _file_sha256( + self.corrected_urdf_path + ), + } + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + cross_view_roll_metrics=getattr( + self, "cross_view_roll_metrics", {} + ), + joint_dynamic_diagnostics=getattr( + self, "joint_dynamic_diagnostics", {} + ), + zero_geometry_diagnostics=( + { + "training_cycles": self.zero_result.training_cycles, + "validation_cycle": self.zero_result.validation_cycle, + "axis_line_rms_m": self.zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": ( + self.zero_result.validation_line_error_by_joint_m + ), + "observability_rank": ( + self.zero_result.observability_rank + ), + "observability_parameter_count": ( + self.zero_result.observability_parameter_count + ), + "observability_condition_number": ( + self.zero_result.observability_condition_number + ), + "offset_covariance_rad2": ( + self.zero_result.offset_covariance_rad2 + ), + } + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + ) + # The JSON is the commit marker for the URDF/JSON pair. It is + # atomically renamed only after the corrected URDF and all three + # artifact hashes have been produced and schema-validated. + atomic_write_json(self.final_path, payload) + except Exception: + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + rejected = self.corrected_urdf_path.with_suffix( + ".urdf.rejected" + ) + self.corrected_urdf_path.replace(rejected) + self.corrected_urdf_path = None + raise self.completed_payload = payload self.state = STATE_COMPLETE self.reason = "calibration_passed" if passed else "quality_failed" @@ -3278,31 +8156,40 @@ class G20ThreeCameraCalibrationNode(Node): self.position_hold_since = None if self.state == STATE_RETURN_BASELINE: self._reset_motion_progress(now, self._baseline_error_u8()) - self._publish_speed_profile(self._normal_speed_profile()) - self._publish_command( - list( - G20ThreeCameraCalibrationNode._current_return_command(self) - ) - ) - elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - self._reset_motion_progress( - now, - self._motion_command_error_u8( - self.active_sweep.spec, - self.active_sweep.start_u8, - ), + return_command = ( + G20ThreeCameraCalibrationNode._current_return_command(self) ) self._publish_speed_profile( - self._speed_profile_for_spec(self.active_sweep.spec) - ) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, return_command ) ) + self._publish_command( + list(return_command) + ) + elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: + preparation_command = getattr( + self, + "preparation_command_u8", + tuple( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ), + ) + self._reset_motion_progress( + now, + self._command_vector_error_u8(preparation_command), + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, preparation_command + ) + ) + self._publish_command(list(preparation_command)) self.reason = f"automatic_retry_{reason}" def _retry_validation_or_pause(self, reason: str, now: float) -> None: @@ -3310,7 +8197,7 @@ class G20ThreeCameraCalibrationNode(Node): self._pause(reason) return item = self.active_validation - key = (item.spec.motor_index, item.command_u8) + key = (_sweep_storage_key(item.spec), item.command_u8) retries = self.validation_retry_counts.get(key, 0) retry_limit = self.automatic_sweep_retry_limit if retries >= retry_limit: @@ -3318,9 +8205,10 @@ class G20ThreeCameraCalibrationNode(Node): return retries += 1 self.validation_retry_counts[key] = retries - runtime = self.views[item.spec.view] - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 + for view in _sweep_views(_node_profile(self), item.spec): + runtime = self.views[view] + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 self.validation_frames_buffer.clear() self.validation_stage_started_at = now self._reset_motion_progress( @@ -3335,7 +8223,7 @@ class G20ThreeCameraCalibrationNode(Node): "kind": "automatic_validation_retry", "view": item.spec.view, "motor_index": item.spec.motor_index, - "command_u8": item.command_u8, + "requested_command_u8": item.command_u8, "reason": reason, "retry": retries, "retry_limit": retry_limit, @@ -3357,7 +8245,9 @@ class G20ThreeCameraCalibrationNode(Node): try: self._advance(now) except Exception as error: - self.get_logger().error(f"Calibration paused: {error}") + self.get_logger().error( + f"Calibration paused: {error}\n{traceback.format_exc()}" + ) self._pause(str(error)) if now - self.last_status_publish >= 0.5: self.last_status_publish = now @@ -3365,14 +8255,21 @@ class G20ThreeCameraCalibrationNode(Node): def _advance(self, now: float) -> None: if self.state == STATE_PREFLIGHT: - if self._all_preflight_ready(now): - self.state = STATE_WAIT_START - self.reason = "call_start" + if not self.started: + if self._all_devices_ready(now): + self.state = STATE_WAIT_START + self.reason = "call_start_for_baseline_recovery" + elif self.startup_baseline_recovered and self._all_preflight_ready(now): + locker = getattr(self, "_lock_fixed_base_references", None) + if locker is None or locker(): + self._start_next_sweep() + else: + self.reason = "locking_fixed_base_references" return if self.state == STATE_WAIT_START: - if not self._all_preflight_ready(now): + if not self._all_devices_ready(now): self.state = STATE_PREFLIGHT - self.reason = "preflight_lost" + self.reason = "device_preflight_lost" return if self.state == STATE_RETURN_BASELINE: baseline_reached = self._baseline_reached() @@ -3391,6 +8288,29 @@ class G20ThreeCameraCalibrationNode(Node): self._retry_motion_or_pause("return_baseline_timeout", now) return if baseline_reached: + return_waypoints = getattr( + self, "return_waypoints", deque() + ) + if return_waypoints: + # Intermediate avoidance waypoints need confirmed feedback, + # but only the final baseline needs the dedicated 0.5 s + # backlash hold. This removes several seconds of no-op + # waiting from every multi-finger return sequence. + self.return_command_u8 = return_waypoints.popleft() + self.position_hold_since = None + self.motion_stage_started_at = now + self._reset_motion_progress( + now, self._baseline_error_u8() + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.return_command_u8 + ) + ) + self._publish_command( + list(self.return_command_u8) + ) + return if self.position_hold_since is None: self.position_hold_since = now elif now - self.position_hold_since >= self.baseline_hold_seconds: @@ -3399,6 +8319,8 @@ class G20ThreeCameraCalibrationNode(Node): self.position_hold_since = None if after in { "next_sweep", + "next_cycle", + "next_task_same_finger", "resume_sweep", "retry_sweep", }: @@ -3407,46 +8329,124 @@ class G20ThreeCameraCalibrationNode(Node): self._fit_all_curves() elif after in {"validation_next", "resume_validation"}: self._start_next_validation() + elif after == "combination_next": + self._start_next_combination_validation() elif after == "finalize": self._finalize() + elif after == "startup_tag_preflight": + # Discard observations and PnP branches from the + # interrupted pose. The normal preflight then requires + # a fresh window of all three fixed palm Tags while the + # hand is confirmed at baseline. + self._finish_startup_baseline_recovery() + elif after == "abort": + self.state = STATE_ABORTED + self.reason = ( + "safe_abort_complete:" + f"{self.abort_original_reason or 'operator_abort'}" + ) else: self.position_hold_since = None return if self.state == STATE_PREPARE_SWEEP: assert self.active_sweep is not None - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._retry_motion_or_pause("sweep_start_position_timeout", now) - return - reached = self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.start_u8, + preparation_command = getattr( + self, + "preparation_command_u8", + tuple( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ), ) + reached = self._command_vector_reached(preparation_command) + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._retry_motion_or_pause( + ( + "sweep_start_tag_timeout" + if reached + else "sweep_start_position_timeout" + ), + now, + ) + return prepare_context = ( f"prepare_motor_{self.active_sweep.spec.motor_index}" ) - prepare_details = self._motion_command_error_details( - self.active_sweep.spec, - self.active_sweep.start_u8, - prepare_context, + prepare_details = self._command_vector_error_details( + preparation_command, prepare_context ) if ( not reached and self._pause_if_motion_stalled( now=now, - error_u8=float(prepare_details["error_u8"]), + error_u8=self._command_vector_error_u8( + preparation_command + ), context=prepare_context, details=prepare_details, ) ): return + preparation_waypoints = getattr( + self, "preparation_waypoints", deque() + ) + if reached and preparation_waypoints: + self.preparation_command_u8 = ( + preparation_waypoints.popleft() + ) + self.motion_stage_started_at = now + self.position_hold_since = None + self._reset_motion_progress( + now, + self._command_vector_error_u8( + self.preparation_command_u8 + ), + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.preparation_command_u8 + ) + ) + self._publish_command(list(self.preparation_command_u8)) + return + if reached: + # Pose-entry waypoints use conservative joint-class speeds. + # Switch to the accepted task scan speed only after the full + # start pose has been reached, then honour the SDK settle time. + self._publish_speed_profile( + self._speed_profile_for_spec(self.active_sweep.spec) + ) speed_ready = bool( now - self.speed_commanded_at >= self.speed_setting_settle_seconds ) - # Do not leave the start endpoint until at least one complete, - # timestamp-synchronised Tag/state observation has been retained. - # The retained frame becomes the strict 0/255 endpoint bin. - if reached and speed_ready and self.sweep_start_frames: + # Do not leave the start endpoint until every contributing camera + # has enough timestamp-synchronised observations. The retained + # frames become the strict 0/255 endpoint bin and, in the first + # training round, the first steady command checkpoint. + minimum_start_frames = ( + 3 + if _steady_checkpoint_commands( + _node_profile(self), self.active_sweep + ) + else 1 + ) + start_frames_ready = _frames_cover_sweep_joints( + self.sweep_start_frames, + self.active_sweep.spec, + minimum_start_frames, + ) + if reached and speed_ready and not start_frames_ready: + self.reason = "waiting_for_task_tags_at_sweep_start" + if ( + reached + and speed_ready + and start_frames_ready + ): if self.position_hold_since is None: self.position_hold_since = now elif now - self.position_hold_since >= self._active_endpoint_hold_seconds(): @@ -3464,25 +8464,69 @@ class G20ThreeCameraCalibrationNode(Node): if now - self.sweep_started_at > self._active_sweep_timeout_seconds(): self._retry_active_sweep_or_pause("sweep_timeout") return - if now - self.sweep_last_valid_at > self.invalid_timeout_seconds: + stale_views = [ + view + for view in _sweep_views( + self.profile, self.active_sweep.spec + ) + if now + - getattr(self, "sweep_last_valid_at_by_view", {}).get( + view, self.sweep_started_at + ) + > self.invalid_timeout_seconds + ] + if stale_views: self._retry_active_sweep_or_pause( - "synchronised_tag_state_timeout" + "synchronised_tag_state_timeout:" + + ",".join(stale_views) ) return - values = [ - float(frame.state_u8[motor]) for frame in self.sweep_frames - ] - span = 0.0 if not values else max(values) - min(values) - target_reached = self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.target_u8, + checkpoint_target_value = getattr( + self, "sweep_checkpoint_target_u8", None + ) + checkpoint_active = bool( + getattr(self, "sweep_checkpoint_mode", False) + and checkpoint_target_value is not None + and not getattr(self, "sweep_baseline_pending", False) + ) + motion_target_u8 = ( + int(self.baseline_command[motor]) + if getattr(self, "sweep_baseline_pending", False) + else int( + checkpoint_target_value + if checkpoint_target_value is not None + else self.active_sweep.target_u8 + ) + ) + target_reached = ( + self._steady_checkpoint_reached( + self.active_sweep.spec, motion_target_u8 + ) + if checkpoint_active + else self._motion_command_reached( + self.active_sweep.spec, motion_target_u8 + ) + ) + sweep_context = ( + f"sweep_baseline_motor_{motor}" + if getattr(self, "sweep_baseline_pending", False) + else f"sweep_motor_{motor}" ) - sweep_context = f"sweep_motor_{motor}" sweep_details = self._motion_command_error_details( self.active_sweep.spec, - self.active_sweep.target_u8, + motion_target_u8, sweep_context, ) + if ( + checkpoint_active + and int(sweep_details.get("motor_index", -1)) == motor + ): + sweep_details["tolerance_u8"] = max( + float( + self.steady_checkpoint_command_feedback_tolerance_u8 + ), + float(sweep_details["tolerance_u8"]), + ) if ( not target_reached and self._pause_if_motion_stalled( @@ -3493,6 +8537,103 @@ class G20ThreeCameraCalibrationNode(Node): ) ): return + if getattr(self, "sweep_baseline_pending", False): + if target_reached: + if self.sweep_baseline_hold_since is None: + # Frames acquired during deceleration into 127 are not + # static observations. Wait baseline_hold_seconds + # after feedback first reaches 127, then collect a + # fresh multi-frame baseline sample. + self.sweep_baseline_frames.clear() + self.sweep_baseline_hold_since = now + elif ( + now - self.sweep_baseline_hold_since + >= self.baseline_hold_seconds + and _frames_cover_sweep_joints( + self.sweep_baseline_frames, + self.active_sweep.spec, + self.minimum_baseline_hold_frames, + ) + ): + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None + self.sweep_endpoint_since = None + self.reason = ( + "collecting_timestamp_synchronised_tag_centres" + ) + if getattr(self, "sweep_checkpoint_mode", False): + self._publish_next_checkpoint(now) + else: + self._reset_motion_progress( + now, + self._motion_command_error_u8( + self.active_sweep.spec, + self.active_sweep.target_u8, + ), + ) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.target_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + else: + self.sweep_baseline_hold_since = None + self.sweep_baseline_frames.clear() + return + if getattr(self, "sweep_checkpoint_mode", False): + checkpoint_target = self.sweep_checkpoint_target_u8 + if checkpoint_target is None: + if self.sweep_checkpoint_commands: + # Recover a missing in-flight target by publishing the + # next queued checkpoint instead of aborting a healthy + # hardware session. + self._publish_next_checkpoint(now) + return + self.sweep_checkpoint_mode = False + else: + if target_reached: + if self.sweep_checkpoint_hold_since is None: + self.sweep_checkpoint_frames.clear() + self.sweep_checkpoint_hold_since = now + elif ( + now - self.sweep_checkpoint_hold_since + >= self.endpoint_hold_seconds + and _frames_cover_sweep_joints( + self.sweep_checkpoint_frames, + self.active_sweep.spec, + 3, + ) + ): + if not self._steady_checkpoint_feedback_is_stable( + self.active_sweep.spec, + self.sweep_checkpoint_frames, + ): + # The command/feedback offset is acceptable, + # but feedback is still settling inside that + # band. Restart the hold window without + # issuing another command or weakening the + # mechanical-stall gate. + self.sweep_checkpoint_hold_since = now + self.sweep_checkpoint_frames.clear() + return + self._record_command_checkpoint( + self.active_sweep, + int(checkpoint_target), + list(self.sweep_checkpoint_frames), + ) + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + if self._publish_next_checkpoint(now): + return + self.sweep_endpoint_since = now + else: + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + if self.sweep_checkpoint_target_u8 is not None: + return if target_reached: if self.sweep_endpoint_since is None: self.sweep_endpoint_since = now @@ -3502,12 +8643,54 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_endpoint_since is not None and now - self.sweep_endpoint_since >= self._active_endpoint_hold_seconds() - and len(self.sweep_frames) >= self.minimum_sweep_frames - and span >= self.minimum_state_span_u8 + and _frames_cover_sweep_motion( + self.sweep_frames, + self.active_sweep.spec, + motor_index=motor, + minimum_per_joint=self.minimum_sweep_frames, + minimum_span_u8=self.minimum_state_span_u8, + ) ): self._finish_active_sweep() return if self.state == STATE_VALIDATION_MOVE: + if self.active_combination_validation is not None: + command = self.active_combination_validation.command_u8 + reached = self._command_vector_reached(command) + details = self._command_vector_error_details( + command, + f"combination_{self.active_combination_validation.name}", + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(details["error_u8"]), + context=str(details["stage"]), + details=details, + ) + ): + return + if reached: + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.endpoint_hold_seconds: + self.position_hold_since = None + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.validation_stage_started_at = now + self.state = STATE_VALIDATION_CAPTURE + self.reason = ( + "combination_validation_capture_" + f"{self.active_combination_validation.name}" + ) + else: + self.position_hold_since = None + if now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_combination_validation_or_pause( + "combination_validation_move_timeout", now + ) + return assert self.active_validation is not None reached = self._motion_command_reached( self.active_validation.spec, @@ -3551,7 +8734,22 @@ class G20ThreeCameraCalibrationNode(Node): self._retry_validation_or_pause("validation_move_timeout", now) return if self.state == STATE_VALIDATION_CAPTURE: - if len(self.validation_frames_buffer) >= self.validation_frames: + if self.active_combination_validation is not None: + if all( + len(frames) >= self.combination_validation_frames + for frames in self.combination_validation_frames_buffer.values() + ): + self._finish_combination_validation_capture() + elif now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_combination_validation_or_pause( + "combination_validation_capture_timeout", now + ) + return + if _frames_cover_sweep_joints( + self.validation_frames_buffer, + self.active_validation.spec, + self.validation_frames, + ): self._finish_validation_capture() elif now - self.validation_stage_started_at > self.validation_timeout_seconds: self._retry_validation_or_pause( @@ -3562,8 +8760,14 @@ class G20ThreeCameraCalibrationNode(Node): views: dict[str, dict[str, Any]] = {} for name, runtime in self.views.items(): required_roles = set(self._required_roles_for_view(name)) + locked_role = self._locked_base_role_for_active_capture(name) + locked_roles = set(() if locked_role is None else (locked_role,)) views[name] = { "ready": self._view_ready(runtime, now), + "stream_alive": bool( + runtime.last_message_at > 0.0 + and now - runtime.last_message_at <= 1.0 + ), "camera_info_valid": runtime.camera_info_valid, "camera_extrinsics_valid": runtime.extrinsics_valid, "detection_hz": round(runtime.detection_hz, 2), @@ -3571,6 +8775,18 @@ class G20ThreeCameraCalibrationNode(Node): "required_tag_ids": sorted( runtime.view_tags[role] for role in required_roles ), + "locked_reference_tag_ids": sorted( + runtime.view_tags[role] for role in locked_roles + ), + "configured_tag_ids": sorted(runtime.view_tags.values()), + "visible_configured_tag_ids": sorted( + tag_id + for role, tag_id in runtime.view_tags.items() + if role in runtime.latest_tag_quality + and self._quality_valid( + runtime.latest_tag_quality[role], include_pnp=False + ) + ), "detected_tag_ids": sorted( tag_id for tag_id, role in runtime.role_by_id.items() @@ -3581,6 +8797,7 @@ class G20ThreeCameraCalibrationNode(Node): tag_id for tag_id, role in runtime.role_by_id.items() if role in required_roles + and role not in locked_roles and role not in runtime.latest_tag_quality ), "pnp_rejections": dict(runtime.latest_pnp_rejections), @@ -3620,6 +8837,14 @@ class G20ThreeCameraCalibrationNode(Node): for value in values } ) + adjacent_bin_gaps = [ + (right - left, left, right) + for left, right in zip(bins, bins[1:]) + ] + maximum_bin_gap, gap_start_u8, gap_end_u8 = max( + adjacent_bin_gaps, + default=(0, None, None), + ) missing_endpoints = [ endpoint for endpoint in (0, 255) if endpoint not in bins ] @@ -3637,6 +8862,34 @@ class G20ThreeCameraCalibrationNode(Node): 1.0, ) ) + storage_key = _sweep_storage_key(self.active_sweep.spec) + frame_counts = { + name: len(_frames_for_joint(self.sweep_frames, name)) + for name in self.active_sweep.spec.joints + } + baseline_frame_counts = { + name: len( + _frames_for_joint(self.sweep_baseline_frames, name) + ) + for name in self.active_sweep.spec.joints + } + detection_rates_by_view = { + view: ( + 0.0 + if getattr( + self, "sweep_detection_total_by_view", {} + ).get(view, 0) <= 0 + else getattr( + self, "sweep_detection_valid_by_view", {} + ).get(view, 0) + / getattr( + self, "sweep_detection_total_by_view", {} + )[view] + ) + for view in _sweep_views( + self.profile, self.active_sweep.spec + ) + } active = { "kind": "sweep", "view": self.active_sweep.spec.view, @@ -3645,11 +8898,14 @@ class G20ThreeCameraCalibrationNode(Node): "cycle": self.active_sweep.cycle + 1, "repetitions": self.repetitions, "direction": self.active_sweep.direction, - "fit_attempt": self.sweep_attempts.get(motor, 1), + "task_name": self.active_sweep.spec.key, + "fit_attempt": self.sweep_attempts.get( + storage_key, 1 + ), "fit_attempt_limit": self.automatic_fit_retry_limit + 1, "automatic_retry_count": self.sweep_retry_counts.get( ( - motor, + storage_key, self.active_sweep.cycle, self.active_sweep.direction, ), @@ -3660,7 +8916,7 @@ class G20ThreeCameraCalibrationNode(Node): 1.0 if self.sweep_retry_counts.get( ( - motor, + storage_key, self.active_sweep.cycle, self.active_sweep.direction, ), @@ -3671,7 +8927,7 @@ class G20ThreeCameraCalibrationNode(Node): min( self.sweep_retry_counts[ ( - motor, + storage_key, self.active_sweep.cycle, self.active_sweep.direction, ) @@ -3689,16 +8945,67 @@ class G20ThreeCameraCalibrationNode(Node): ), "start_u8": self.active_sweep.start_u8, "target_u8": self.active_sweep.target_u8, + "current_motion_target_u8": ( + int( + getattr( + self, + "preparation_command_u8", + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ), + )[motor] + ) + if self.state == STATE_PREPARE_SWEEP + else int(self.baseline_command[motor]) + if getattr(self, "sweep_baseline_pending", False) + else int( + self.sweep_checkpoint_target_u8 + if getattr(self, "sweep_checkpoint_target_u8", None) + is not None + else self.active_sweep.target_u8 + ) + ), + "steady_checkpoint_mode": bool( + getattr(self, "sweep_checkpoint_mode", False) + ), + "remaining_steady_checkpoints": len( + getattr(self, "sweep_checkpoint_commands", ()) + ), + "baseline_hold_pending": bool( + getattr(self, "sweep_baseline_pending", False) + ), + "baseline_hold_valid_frames": min( + baseline_frame_counts.values(), default=0 + ), + "minimum_baseline_hold_frames": int( + getattr(self, "minimum_baseline_hold_frames", 10) + ), "actual_u8": actual, "motion_progress": motion_progress, - "valid_frames": len(self.sweep_frames), + "valid_frames": min(frame_counts.values(), default=0), + "valid_frames_by_joint": frame_counts, + "detection_frames": int( + getattr(self, "sweep_detection_total_frames", 0) + ), + "detection_valid_frames": int( + getattr(self, "sweep_detection_valid_frames", 0) + ), + "detection_rate": min( + detection_rates_by_view.values(), default=0.0 + ), + "detection_rate_by_view": detection_rates_by_view, "sample": { "minimum_u8": None if not values else min(values), "maximum_u8": None if not values else max(values), "span_u8": 0.0 if not values else max(values) - min(values), "bin_count": len(bins), "minimum_bin_count": self.minimum_sweep_bins, - "maximum_bin_gap": int(max(np.diff(bins), default=0)), + "maximum_bin_gap": int(maximum_bin_gap), + "maximum_bin_gap_start_u8": gap_start_u8, + "maximum_bin_gap_end_u8": gap_end_u8, "allowed_maximum_bin_gap": self.maximum_bin_gap, "missing_endpoint_u8": missing_endpoints, "endpoint_tolerance_u8": max( @@ -3729,6 +9036,11 @@ class G20ThreeCameraCalibrationNode(Node): "normal_speed": self.normal_calibration_speed, "index_roll_speed": self.index_roll_calibration_speed, "index_flex_speed": self.index_flex_calibration_speed, + "adaptive_formal_speed_scale": float( + self.formal_speed_scales.get( + self.active_sweep.spec.key, 1.0 + ) + ), }, } elif self.active_validation is not None: @@ -3766,28 +9078,78 @@ class G20ThreeCameraCalibrationNode(Node): "normal_speed": self.normal_calibration_speed, "index_roll_speed": self.index_roll_calibration_speed, "index_flex_speed": self.index_flex_calibration_speed, + "adaptive_formal_speed_scale": float( + self.formal_speed_scales.get( + self.active_validation.spec.key, 1.0 + ) + ), }, } elif self.fit_failure: active = dict(self.fit_failure) + group_pnp_reasons = { + name: runtime.latest_group_pnp_reason + for name, runtime in self.views.items() + if runtime.latest_group_pnp_reason + } + if group_pnp_reasons: + active = dict(active) + active["group_pnp_reasons"] = group_pnp_reasons + resumed_tasks = set(self.resumed_task_keys) + completed_sweep_count = sum( + 1 + for index, item in enumerate(self.sweep_items) + if index < self.sweep_index or item.spec.key in resumed_tasks + ) scan_progress = ( 0.0 if not self.sweep_items else float( - np.clip(self.sweep_index / len(self.sweep_items), 0.0, 1.0) + np.clip( + completed_sweep_count / len(self.sweep_items), 0.0, 1.0 + ) ) ) + status_validation_index = ( + self.combination_validation_index + if self.combination_validation_items + else self.validation_index + ) + status_validation_total = ( + len(self.combination_validation_items) + if self.combination_validation_items + else len(self.validation_items) + ) payload = { + "serial_number": self.serial_number, + "session_dir": str(self.session_dir), "hand_type": self.hand_type, "reference_finger": self.profile.reference_finger, "state": self.state, "reason": self.reason, + "preflight_mode": ( + "recovering_baseline" + if self.state == STATE_RETURN_BASELINE + and self.baseline_after == "startup_tag_preflight" + else ( + "device_only_before_baseline" + if self.state in {STATE_PREFLIGHT, STATE_WAIT_START} + and not self.started + else ( + "baseline_tags_after_recovery" + if self.state == STATE_PREFLIGHT + and self.started + and self.startup_baseline_recovered + else "" + ) + ) + ), "progress": round( _overall_progress( self.state, scan_progress=scan_progress, - validation_index=self.validation_index, - validation_total=len(self.validation_items), + validation_index=status_validation_index, + validation_total=status_validation_total, ), 4, ), @@ -3802,6 +9164,20 @@ class G20ThreeCameraCalibrationNode(Node): len(self.latest_state_u8) == 20 and now - self.last_state_at <= 1.0 ), + "feedback_hz": round( + ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] + <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / ( + self.state_receive_times[-1] + - self.state_receive_times[0] + ) + ), + 2, + ), "views": views, "active": active, "result_path": ( @@ -3813,9 +9189,50 @@ class G20ThreeCameraCalibrationNode(Node): else str(self.corrected_urdf_path) ), "camera_extrinsics_error": self.extrinsics_error, + "resume": { + "used": bool(self.resumed_task_keys), + "source_session": self.resume_source_session, + "completed_task_count": len(self.resumed_task_keys), + "total_task_count": len(self.profile.sweep_specs), + "completed_task_keys": list(self.resumed_task_keys), + }, "quality": ( {} if self.completed_payload is None else self.completed_payload["quality"] ), + "combination_validation": { + "enabled": bool(self.combination_validation_enabled), + "completed": bool(self.combination_validation_completed), + "completed_poses": int(self.combination_validation_index), + "total_poses": len(self.combination_validation_items), + "position_p95_m": ( + None + if not self.combination_position_errors_m + else round( + float( + np.percentile( + self.combination_position_errors_m, 95.0 + ) + ), + 8, + ) + ), + "orientation_p95_rad": ( + None + if not self.combination_orientation_errors_rad + else round( + float( + np.percentile( + self.combination_orientation_errors_rad, 95.0 + ) + ), + 8, + ) + ), + **combination_target_coverage( + self.combination_tag_observation_counts, + self.combination_tag_validation_counts, + ), + }, } status = String() status.data = json.dumps(payload, ensure_ascii=False) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py index f6721fb..67b7f42 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py @@ -634,6 +634,7 @@ def _fit_joint_curve( values: Sequence[float], *, endpoint_reference: Mapping[str, Sequence[float]] | None = None, + preserve_direction_offset: bool = False, ) -> tuple[dict[str, Any], float, float]: by_direction: dict[str, list[list[float]]] = { direction: [[] for _ in range(256)] for direction in DIRECTIONS @@ -669,9 +670,11 @@ def _fit_joint_curve( ], dtype=float, ) - raw -= raw[-1] + if not preserve_direction_offset: + raw -= raw[-1] projected_samples = isotonic_nonincreasing(raw) - projected_samples -= projected_samples[-1] + if not preserve_direction_offset: + projected_samples -= projected_samples[-1] maximum_correction = max( maximum_correction, float(np.max(np.abs(projected_samples - raw))), @@ -681,7 +684,8 @@ def _fit_joint_curve( commands.astype(float), projected_samples, ) - curve -= curve[255] + if not preserve_direction_offset: + curve -= curve[255] if endpoint_reference is not None: curve = _regularize_coupled_zero_tail( curve, @@ -695,7 +699,8 @@ def _fit_joint_curve( increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float) hysteresis = float(np.max(np.abs(decreasing - increasing))) combined = 0.5 * (decreasing + increasing) - combined -= combined[255] + if not preserve_direction_offset: + combined -= combined[255] return ( { "angle_rad": [round(float(value), 8) for value in combined], 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 051a116..bfc582d 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 @@ -8,17 +8,21 @@ import math import os from pathlib import Path import re +import shutil from typing import Any, Mapping, Sequence import xml.etree.ElementTree as ET import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation +from scipy.stats import t as student_t from .core import fit_rotation_axis, robust_rotation_summary from .full_hand import ( + G20_RIGHT_19_LAYOUT, IMAGE_TRAJECTORY_JOINTS, LEFT_HAND_PROFILE, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, HandCalibrationProfile, JointCurveFit, get_hand_calibration_profile, @@ -54,6 +58,8 @@ class ZeroCalibrationProfile: def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: + if hand.layout_id == G20_RIGHT_19_LAYOUT: + return _build_right_19_zero_profile(hand) reference = hand.reference_finger reference_roll = f"{reference}_mcp_roll" reference_pitch = f"{reference}_mcp_pitch" @@ -100,10 +106,6 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: reference_pitch: 0.0, reference_pip: 0.0, } - # No per-device or per-side numeric calibration belongs in the profile. - # Any non-zero thumb CMC origin must come from this session's measured - # trajectories and pass the independent-cycle validation below. - static_output_zero_offsets: dict[str, float] = {} return ZeroCalibrationProfile( hand=hand, direct_zero_joints=direct, @@ -127,28 +129,108 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: axis_parent_joint=axis_parent, phase_parent_joint=phase_parent, offset_observer_joint=observer, - # The active MCP trajectory is observable, but its absolute static - # phase is inferred only through the passive thumb-IP axis-line centre. - # In the fixed front monocular view that shallow arc has a 37-80 degree - # rotation/circle-axis disagreement and a repeatable phase bias. It is - # therefore not an admissible absolute-zero observation. Keep the - # original CAD zero while still measuring and validating angle_rad[256]. - # The current observations recover the reference finger's dynamic - # curves, but the physical four-finger straight pose is already the CAD - # datum. With only one of the four fingers tagged, writing its fitted - # absolute MCP/PIP phase (or copying it to independent motors) can bend - # one finger or tilt all four even when the real hand is straight. - # Keep every four-finger static origin at source CAD zero; inherit only - # the measured command-angle curve. thumb_mcp likewise depends on a - # shallow passive-IP circle whose absolute phase is not trustworthy. - # Stable systematic geometry bias must not be written as encoder zero. fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, - static_output_zero_offsets_rad=static_output_zero_offsets, + static_output_zero_offsets_rad={}, ) -def get_zero_calibration_profile(side: str) -> ZeroCalibrationProfile: - return _build_zero_profile(get_hand_calibration_profile(side)) +def _build_right_19_zero_profile( + hand: HandCalibrationProfile, +) -> ZeroCalibrationProfile: + """Return the 12-observable-static-zero graph for the 15-Tag layout.""" + fingers = ("index", "middle", "ring", "pinky") + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch") + ), + ) + axis_joints = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch", "pip") + ), + ) + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + **{ + f"{finger}_mcp_pitch": f"{finger}_mcp_roll" + for finger in fingers + }, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + **{ + f"{finger}_{child}": f"{finger}_{parent}" + for finger in fingers + for child, parent in (("pip", "mcp_pitch"),) + }, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + **{ + f"{finger}_{target}": f"{finger}_{observed}" + for finger in fingers + for target, observed in ( + ("mcp_roll", "mcp_pitch"), + ("mcp_pitch", "pip"), + ) + }, + } + root_anchors = frozenset( + {"thumb_cmc_roll", *(f"{finger}_mcp_roll" for finger in fingers)} + ) + constrained = frozenset( + set(hand.image_trajectory_joints) + | {"thumb_cmc_yaw"} + # The side camera observes all four flexion axes nearly end-on. A + # 16 mm monocular planar Tag gives a very repeatable orientation axis, + # while its command-correlated depth bias can tilt the otherwise clean + # centre-trajectory plane by several degrees. Use the orientation + # axis to constrain that circle; PIP retains its independently fitted + # axis line and all final SE(3)/holdout quality gates. + | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=tuple(direct), + axis_joints=tuple(axis_joints), + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=constrained, + root_anchor_joints=root_anchors, + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + # Keep the commit-proven thumb reference convention: the passive-IP + # observation is sufficiently good for the CMC/MCP trajectory, but + # not for redefining the MCP CAD zero. Optimising this value made the + # whole downstream thumb chain move during otherwise good CMC fits. + fixed_direct_zero_offsets_rad={"thumb_mcp": 0.0}, + static_output_zero_offsets_rad={}, + ) + + +def get_zero_calibration_profile( + side: str, layout_id: str = "legacy_11" +) -> ZeroCalibrationProfile: + return _build_zero_profile(get_hand_calibration_profile(side, layout_id)) LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) @@ -166,6 +248,14 @@ OPTIMIZED_ZERO_JOINTS = tuple( AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) + +def circle_direction_is_constrained( + joint_name: str, constrained_circle_joints: frozenset[str] +) -> bool: + """Match the constraint used by both canonical and side aliases.""" + name = str(joint_name) + return name in constrained_circle_joints or name.endswith("_side") + ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 # Fit an axis-line point from the complete relative SE(3) trajectory instead @@ -283,6 +373,27 @@ def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: return record.get("cycle"), record.get("direction") +def _canonical_reference_records( + records: Sequence[Mapping[str, Any]], + canonical_zero_direction: str | None, +) -> list[Mapping[str, Any]]: + """Select the sole physical-zero branch when one is configured.""" + if canonical_zero_direction is None: + return list(records) + if canonical_zero_direction not in {"decreasing", "increasing"}: + raise ValueError("canonical zero direction is invalid") + selected = [ + record + for record in records + if str(record.get("direction")) == canonical_zero_direction + ] + if not selected: + raise ValueError( + f"canonical {canonical_zero_direction} zero branch has no samples" + ) + return selected + + def _interpolate_reference_rotation( records: Sequence[Mapping[str, Any]], zero_command_u8: int ) -> Rotation | None: @@ -366,10 +477,73 @@ def _baseline_reference( return robust_rotation_summary(values)[0] +def baseline_hysteresis_by_cycle_rad( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_xyz: Sequence[float] | None = None, +) -> tuple[float, ...]: + """Return decreasing/increasing joint-angle disagreement per cycle. + + Full SO(3) disagreement includes planar-PnP tilt noise that is orthogonal + to the fitted revolute axis. When an axis is supplied, report only the + physically meaningful component about that axis; the orthogonal component + remains covered by the independent rotation-model residual gate. + """ + samples = [dict(record) for record in records] + axis: np.ndarray | None = None + if axis_xyz is not None: + axis = _vector(axis_xyz, 3, name="baseline hysteresis axis") + norm = float(np.linalg.norm(axis)) + if norm <= 0.0: + raise ValueError("baseline hysteresis axis must be non-zero") + axis /= norm + result: list[float] = [] + for cycle in sorted({int(record["cycle"]) for record in samples}): + rotations: dict[str, Rotation] = {} + for direction in ("decreasing", "increasing"): + selected = [ + record + for record in samples + if int(record["cycle"]) == cycle + and str(record["direction"]) == direction + ] + reference = _interpolate_reference_rotation( + selected, zero_command_u8 + ) + if reference is None: + raise ValueError( + f"cycle {cycle} {direction} is missing baseline samples" + ) + rotations[direction] = reference + delta = ( + rotations["decreasing"].inv() + * rotations["increasing"] + ) + result.append( + float( + delta.magnitude() + if axis is None + else abs(float(delta.as_rotvec() @ axis)) + ) + ) + if not result: + raise ValueError("baseline hysteresis requires at least one cycle") + return tuple(result) + + def fit_rotation_joint_curve( - records: Sequence[Mapping[str, Any]], *, zero_command_u8: int + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + canonical_zero_direction: str | None = None, ) -> JointCurveFit: - """Fit a command curve from full parent-to-child tag orientations.""" + """Fit a direction-aware curve from parent-to-child Tag orientations. + + With ``canonical_zero_direction`` both branches share one physical + reference. Only the canonical branch is zero at ``zero_command_u8``; + the other branch retains its measured backlash/compliance offset. + """ samples = [dict(record) for record in records] if len(samples) < 12: raise ValueError("rotation trajectory requires at least 12 samples") @@ -382,7 +556,12 @@ def fit_rotation_joint_curve( record for record in samples if int(record["cycle"]) == cycle ] references[cycle] = Rotation.from_quat( - _baseline_reference(cycle_records, zero_command_u8) + _baseline_reference( + _canonical_reference_records( + cycle_records, canonical_zero_direction + ), + zero_command_u8, + ) ) for record in samples: observed = Rotation.from_quat(_relative_rotation(record)) @@ -393,11 +572,26 @@ def fit_rotation_joint_curve( commands.append(int(record["command_u8"])) axis = fit_rotation_axis(vectors, commands) values_by_record = [float(vector @ axis) for vector in vectors] - curves, correction, hysteresis = _fit_joint_curve(samples, values_by_record) - for key in ("angle_rad", "decreasing_rad", "increasing_rad"): - values = np.asarray(curves[key], dtype=float) - values -= float(values[int(zero_command_u8)]) - curves[key] = [round(float(value), 8) for value in values] + curves, correction, hysteresis = _fit_joint_curve( + samples, + values_by_record, + preserve_direction_offset=canonical_zero_direction is not None, + ) + if canonical_zero_direction is None: + for key in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) + values -= float(values[int(zero_command_u8)]) + curves[key] = [round(float(value), 8) for value in values] + else: + canonical_key = f"{canonical_zero_direction}_rad" + shared_zero = float(curves[canonical_key][int(zero_command_u8)]) + for key in ("decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) - shared_zero + curves[key] = [round(float(value), 8) for value in values] + # Before the bridge has observed motion direction it must use the + # same branch that defines the URDF physical zero, never an average + # pose that the mechanism may not be able to occupy. + curves["angle_rad"] = list(curves[canonical_key]) orthogonal = [ float(np.linalg.norm(vector - float(vector @ axis) * axis)) for vector in vectors @@ -412,8 +606,14 @@ def fit_rotation_joint_curve( "zero_command_u8": int(zero_command_u8), "reference_quaternion_xyzw": [ float(value) - for value in _baseline_reference(samples, zero_command_u8) + for value in _baseline_reference( + _canonical_reference_records( + samples, canonical_zero_direction + ), + zero_command_u8, + ) ], + "canonical_zero_direction": canonical_zero_direction, }, maximum_monotonic_correction_rad=float(correction), maximum_hysteresis_rad=float(hysteresis), @@ -449,6 +649,37 @@ def measure_rotation_joint_observation( return float((reference.inv() * observed).as_rotvec() @ axis) +def measure_joint_curve_observation( + fit: JointCurveFit, + *, + quaternion_xyzw: Sequence[float] | None = None, + image_relative_xy_px: Sequence[float] | None = None, +) -> float: + """Measure one observation in the same space as its fitted curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + if quaternion_xyzw is None: + raise ValueError("rotation observation quaternion is missing") + return measure_rotation_joint_observation(fit, quaternion_xyzw) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve observation representation") + if image_relative_xy_px is None: + raise ValueError("image curve observation point is missing") + point = np.asarray(image_relative_xy_px, dtype=float) + centre = np.asarray(fit.circle["center_xy_px"], dtype=float) + reference = np.asarray(fit.circle["reference_xy_px"], dtype=float) + vector = point - centre + if ( + point.shape != (2,) + or not np.all(np.isfinite(point)) + or float(np.linalg.norm(vector)) < 1.0e-9 + ): + raise ValueError("image curve observation point is invalid") + return float(fit.circle["orientation_sign"]) * math.atan2( + float(reference[0] * vector[1] - reference[1] * vector[0]), + float(reference @ vector), + ) + + def rotation_curve_holdout_errors( fit: JointCurveFit, records: Sequence[Mapping[str, Any]], @@ -459,8 +690,12 @@ def rotation_curve_holdout_errors( samples = [dict(record) for record in records] if not samples: raise ValueError("holdout records are empty") + canonical_zero_direction = fit.circle.get("canonical_zero_direction") reference = Rotation.from_quat( - _baseline_reference(samples, zero_command_u8) + _baseline_reference( + _canonical_reference_records(samples, canonical_zero_direction), + zero_command_u8, + ) ) axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") axis /= np.linalg.norm(axis) @@ -479,6 +714,38 @@ def rotation_curve_holdout_errors( return tuple(errors) +def joint_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, +) -> tuple[float, ...]: + """Validate either a rotation or fixed-parent image-circle curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + return rotation_curve_holdout_errors( + fit, records, zero_command_u8=zero_command_u8 + ) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve holdout representation") + if not records: + raise ValueError("holdout records are empty") + errors: list[float] = [] + for record in records: + observed = measure_joint_curve_observation( + fit, + image_relative_xy_px=record["image_relative_xy_px"], + ) + command = int(record["command_u8"]) + direction = str(record["direction"]) + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + errors.append(observed - float(expected_curve[command])) + return tuple(errors) + + @dataclass(frozen=True) class JointAxisMeasurement: joint: str @@ -513,6 +780,8 @@ def _fit_axis_point_from_pose_trajectory( angle_axis_parent_xyz: Sequence[float], phase_reference_point_parent_xyz: Sequence[float], view_normal_common_xyz: Sequence[float] | None, + canonical_zero_direction: str | None = None, + allow_axial_translation: bool = False, ) -> tuple[np.ndarray, float, str]: """Fit the closest point on a revolute axis from full relative poses. @@ -523,14 +792,17 @@ def _fit_axis_point_from_pose_trajectory( treating a monocular Tag-centre depth arc as ground-truth geometry. """ samples = [dict(record) for record in records] - zero_records = _near_zero_records(samples, zero_command_u8) + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) + zero_records = _near_zero_records(reference_samples, zero_command_u8) if not zero_records: raise ValueError( f"axis-point fit has no record near baseline {zero_command_u8}" ) reference_rotation = Rotation.from_quat( - _baseline_reference(samples, zero_command_u8) + _baseline_reference(reference_samples, zero_command_u8) ).as_matrix() reference_translation = np.median( np.asarray( @@ -670,7 +942,16 @@ def _fit_axis_point_from_pose_trajectory( observed_translation - delta_rotation @ reference_translation ) - projection = np.eye(3) + # A screw-driven revolute joint may carry real translation along its + # axis. That component is in the null space of (I - R), contains no + # information about the perpendicular axis-line point, and must not + # inflate or bias the line fit as if the mechanism were pure rotary. + axis_projection = ( + np.eye(3) - np.outer(axis, axis) + if allow_axial_translation + else np.eye(3) + ) + projection = axis_projection axis_point_matrix = (np.eye(3) - delta_rotation) @ basis if use_image_plane_projection: assert view_normal_common is not None @@ -681,9 +962,10 @@ def _fit_axis_point_from_pose_trajectory( view_normal_common ) view_normal_parent /= np.linalg.norm(view_normal_parent) - projection -= np.outer( + image_projection = np.eye(3) - np.outer( view_normal_parent, view_normal_parent ) + projection = image_projection @ axis_projection # A single end-on camera cannot distinguish absolute depth from a # shift along the revolute axis. Keep the observable image-plane # equations only; adding a free baseline-depth variable makes the @@ -739,6 +1021,7 @@ def fit_joint_axis_measurement( axis_common_constraint: Sequence[float] | None = None, constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, view_normal_common_xyz: Sequence[float] | None = None, + canonical_zero_direction: str | None = None, ) -> JointAxisMeasurement: """Fit one physical screw axis from one complete scan cycle.""" samples = [ @@ -759,8 +1042,11 @@ def fit_joint_axis_measurement( else float(singular_values[1] / singular_values[0]) ) + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) reference = Rotation.from_quat( - _baseline_reference(samples, zero_command_u8) + _baseline_reference(reference_samples, zero_command_u8) ) rotation_vectors = [ (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() @@ -779,7 +1065,7 @@ def fit_joint_axis_measurement( float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) ) - zero_records = _near_zero_records(samples, zero_command_u8) + zero_records = _near_zero_records(reference_samples, zero_command_u8) if not zero_records: raise ValueError( f"{joint} cycle has no record near baseline {zero_command_u8}" @@ -842,6 +1128,8 @@ def fit_joint_axis_measurement( angle_axis_parent_xyz=rotation_axis, phase_reference_point_parent_xyz=circle["center_xyz_m"], view_normal_common_xyz=view_normal_common_xyz, + canonical_zero_direction=canonical_zero_direction, + allow_axial_translation=str(joint).endswith("_side"), ) ) point_common = parent_rotation.apply(point_parent) + parent_translation @@ -950,6 +1238,40 @@ class UrdfKinematicModel: result.reverse() return result + def link_transform( + self, + target_joint: str, + *, + zero_offsets: Mapping[str, float], + joint_angles: Mapping[str, float], + independent_mimic_angles: bool = False, + ) -> np.ndarray: + """Return base-to-child-link pose for calibration validation. + + ``independent_mimic_angles`` lets the product validator use the five + independently measured passive JSON curves while the emitted URDF's + original mimic XML remains untouched. + """ + transform = np.eye(4) + resolved = {str(name): float(value) for name, value in joint_angles.items()} + for joint in self._chain(target_joint): + transform = transform @ joint.origin @ _axis_rotation( + joint.axis, float(zero_offsets.get(joint.name, 0.0)) + ) + if independent_mimic_angles and joint.name in resolved: + angle = resolved[joint.name] + elif joint.mimic_joint is None: + angle = float(resolved.get(joint.name, 0.0)) + else: + angle = ( + joint.mimic_multiplier + * float(resolved.get(joint.mimic_joint, 0.0)) + + joint.mimic_offset + ) + resolved[joint.name] = angle + transform = transform @ _axis_rotation(joint.axis, angle) + return transform + def axis_line( self, target_joint: str, @@ -1010,9 +1332,16 @@ class ZeroSolveResult: passed: bool cycle_offsets_rad: Mapping[str, tuple[float, ...]] offset_uncertainty_rad: Mapping[str, float] + offset_confidence_half_width_rad: Mapping[str, float] + training_cycles: tuple[int, ...] + validation_cycle: int validation_original_error_by_joint_rad: Mapping[str, float] validation_improvement_by_joint_rad: Mapping[str, float] validation_improvement_confidence_lower_rad: Mapping[str, float] + observability_rank: int + observability_parameter_count: int + observability_condition_number: float + offset_covariance_rad2: Mapping[str, float] failure_reasons: Mapping[str, str] @@ -1050,9 +1379,13 @@ def solve_urdf_zero_offsets( significance_sigma: float = 3.0, maximum_validation_mae_rad: float = math.radians(1.0), maximum_validation_p95_rad: float = math.radians(2.0), + 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_pose_axis_line_rms_m: float = 0.001, + maximum_observability_condition_number: float = 1.0e10, hand_type: str = "left", + tag_layout: str = "legacy_11", fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, static_output_zero_offsets_rad: Mapping[str, float] | None = None, ) -> ZeroSolveResult: @@ -1065,7 +1398,7 @@ def solve_urdf_zero_offsets( length, along-axis Tag placement, and monocular depth bias from being absorbed as an encoder-zero correction. """ - profile = get_zero_calibration_profile(hand_type) + profile = get_zero_calibration_profile(hand_type, tag_layout) model = UrdfKinematicModel(source_urdf) training = [m for m in measurements if m.cycle in set(training_cycles)] validation = [m for m in measurements if m.cycle == int(validation_cycle)] @@ -1100,6 +1433,20 @@ def solve_urdf_zero_offsets( raise ValueError("maximum axis cone mismatch must be positive") 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: + raise ValueError( + "maximum observability condition number must be greater than one" + ) + if ( + maximum_validation_error_rad is not None + and maximum_validation_error_rad <= 0.0 + ): + raise ValueError("maximum validation error must be positive") + if ( + maximum_confidence_half_width_rad is not None + and maximum_confidence_half_width_rad <= 0.0 + ): + raise ValueError("maximum confidence half width must be positive") def predicted_local( measurement: JointAxisMeasurement, @@ -1168,8 +1515,6 @@ def solve_urdf_zero_offsets( ) zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} zero_offsets.update(fixed_offsets) - generator = np.random.default_rng(20260806) - def fit_base_pose( selected: Sequence[JointAxisMeasurement], ) -> tuple[Rotation, np.ndarray]: @@ -1239,6 +1584,280 @@ def solve_urdf_zero_offsets( axis=0, ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + # In the product layout none of the four finger-roll zeros is a + # mechanical prior. Using one finger's pitch axis to orient the + # palm would therefore absorb that finger's roll zero into the + # palm pose. The named positions of five parallel root lines do + # observe rotation about their common direction, independent of + # all five root-joint zeros. Fit that transverse line pattern and + # leave the unobservable translation along the common direction + # at zero; all zero-sensitive phase residuals use line-to-line + # differences and are invariant to that gauge. + predicted_common = undirected_axis_average( + [predicted_axes[name] for name in root_names] + ) + observed_common = undirected_axis_average( + [observed_axes[name] for name in root_names] + ) + predicted_center = np.mean( + np.asarray([predicted_points[name] for name in root_names]), + axis=0, + ) + observed_center = np.mean( + np.asarray([observed_points[name] for name in root_names]), + axis=0, + ) + predicted_pattern = { + name: ( + (delta := predicted_points[name] - predicted_center) + - predicted_common * float(delta @ predicted_common) + ) + for name in root_names + } + observed_pattern = { + name: ( + (delta := observed_points[name] - observed_center) + - observed_common * float(delta @ observed_common) + ) + for name in root_names + } + if max( + np.linalg.norm(value) for value in predicted_pattern.values() + ) < 0.003: + raise ValueError("root axis-line pattern is degenerate") + if max( + np.linalg.norm(value) for value in observed_pattern.values() + ) < 0.003: + raise ValueError("observed root axis-line pattern is degenerate") + + def align_axis(source: np.ndarray, target: np.ndarray) -> Rotation: + source = source / np.linalg.norm(source) + target = target / np.linalg.norm(target) + cross = np.cross(source, target) + cross_norm = float(np.linalg.norm(cross)) + dot = float(np.clip(source @ target, -1.0, 1.0)) + if cross_norm > 1.0e-10: + return Rotation.from_rotvec( + cross / cross_norm * math.atan2(cross_norm, dot) + ) + if dot > 0.0: + return Rotation.identity() + basis = np.eye(3)[int(np.argmin(np.abs(source)))] + axis = np.cross(source, basis) + axis /= np.linalg.norm(axis) + return Rotation.from_rotvec(axis * math.pi) + + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for sign in (1.0, -1.0): + signed_observed_axis = sign * observed_common + axis_rotation = align_axis( + predicted_common, signed_observed_axis + ) + mapped_pattern = { + name: axis_rotation.apply(predicted_pattern[name]) + for name in root_names + } + cosine = sum( + float(mapped_pattern[name] @ observed_pattern[name]) + for name in root_names + ) + sine = sum( + float( + signed_observed_axis + @ np.cross( + mapped_pattern[name], observed_pattern[name] + ) + ) + for name in root_names + ) + phase_rotation = Rotation.from_rotvec( + signed_observed_axis * math.atan2(sine, cosine) + ) + rotation = phase_rotation * axis_rotation + translation_samples = [] + for name in root_names: + delta = observed_points[name] - rotation.apply( + predicted_points[name] + ) + translation_samples.append( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + translation = np.median( + np.asarray(translation_samples), axis=0 + ) + + # Root-axis points recovered from planar Tags occasionally + # contain a large but repeatable depth/line-position outlier. + # A plain Procrustes sum lets one such point rotate the palm + # frame and then makes the same bias look like a common zero + # offset on all four finger-roll joints. Refine only the + # rigid palm pose with a millimetre-scale robust loss. At + # least three mutually consistent named root lines therefore + # determine the transverse pattern while an outlier remains + # visible in the line diagnostics below. + transverse_helper = np.eye(3)[ + int(np.argmin(np.abs(signed_observed_axis))) + ] + transverse_first = np.cross( + signed_observed_axis, transverse_helper + ) + transverse_first /= np.linalg.norm(transverse_first) + transverse_second = np.cross( + signed_observed_axis, transverse_first + ) + + def robust_root_pattern_residual( + value: np.ndarray, + ) -> np.ndarray: + candidate_rotation = ( + Rotation.from_rotvec( + signed_observed_axis * float(value[0]) + ) + * rotation + ) + candidate_translation = ( + translation + + transverse_first * float(value[1]) + + transverse_second * float(value[2]) + ) + residuals: list[float] = [] + for name in root_names: + delta = observed_points[name] - ( + candidate_rotation.apply(predicted_points[name]) + + candidate_translation + ) + residuals.extend( + ( + float(delta @ transverse_first) / 0.001, + float(delta @ transverse_second) / 0.001, + ) + ) + return np.asarray(residuals, dtype=float) + + root_pattern_solution = least_squares( + robust_root_pattern_residual, + np.zeros(3, dtype=float), + bounds=( + np.asarray([-math.pi, -0.25, -0.25]), + np.asarray([math.pi, 0.25, 0.25]), + ), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not root_pattern_solution.success: + raise ValueError("robust root axis-line pattern fit failed") + rotation = ( + Rotation.from_rotvec( + signed_observed_axis + * float(root_pattern_solution.x[0]) + ) + * rotation + ) + translation = ( + translation + + transverse_first * float(root_pattern_solution.x[1]) + + transverse_second * float(root_pattern_solution.x[2]) + ) + axis_errors: list[float] = [] + for item in selected: + predicted_axis, _ = predicted_local(item, zero_offsets) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + axis_errors.append( + math.acos( + abs( + float( + np.clip( + rotation.apply(predicted_axis) + @ observed_axis, + -1.0, + 1.0, + ) + ) + ) + ) + ) + line_errors = [] + for name in root_names: + predicted_point = ( + rotation.apply(predicted_points[name]) + translation + ) + delta = observed_points[name] - predicted_point + line_errors.append( + float( + np.linalg.norm( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + ) + ) + score = float(np.mean(np.square(axis_errors))) + float( + np.mean(np.square(np.asarray(line_errors) / 0.01)) + ) + candidates.append((score, rotation, translation)) + _, rotation, translation = min( + candidates, key=lambda item: item[0] + ) + non_root_items = [ + item + for item in selected + if item.joint not in profile.root_anchor_joints + ] + if not non_root_items: + raise ValueError( + "non-parallel axes are required for palm axial translation" + ) + + def axial_translation_residual(value: np.ndarray) -> np.ndarray: + candidate_translation = ( + translation + observed_common * float(value[0]) + ) + residuals: list[float] = [] + for item in non_root_items: + predicted_axis, predicted_point = predicted_local( + item, zero_offsets + ) + predicted_axis = rotation.apply(predicted_axis) + predicted_point = ( + rotation.apply(predicted_point) + + candidate_translation + ) + observed_axis = np.asarray( + item.axis_common_xyz, dtype=float + ) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + delta = np.asarray( + item.point_common_xyz_m, dtype=float + ) - predicted_point + perpendicular = delta - observed_axis * float( + delta @ observed_axis + ) + residuals.extend( + float(component) / 0.001 + for component in perpendicular + ) + return np.asarray(residuals, dtype=float) + + axial_solution = least_squares( + axial_translation_residual, + np.asarray([0.0]), + bounds=(np.asarray([-1.0]), np.asarray([1.0])), + loss="soft_l1", + f_scale=1.0, + ) + if not axial_solution.success: + raise ValueError("palm axial translation fit failed") + translation = ( + translation + + observed_common * float(axial_solution.x[0]) + ) + return rotation, translation + predicted_orientation_axis = undirected_axis_average( [predicted_local(item, zero_offsets)[0] for item in orientation_items] ) @@ -1585,6 +2204,99 @@ def solve_urdf_zero_offsets( base_rotation, base_translation, training_offsets = solve_selected(training) + def observability_residual(parameters: np.ndarray) -> np.ndarray: + rotation = Rotation.from_rotvec(parameters[:3]) + translation = parameters[3:6] * 0.05 + offsets = { + name: float(value) + for name, value in zip( + profile.direct_zero_joints, parameters[6:] + ) + } + residuals: list[float] = [] + for item in training: + predicted_axis, predicted_point = predicted_local(item, offsets) + predicted_axis = rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = rotation.apply(predicted_point) + translation + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + predicted_moment = np.cross(predicted_point, predicted_axis) + observed_moment = np.cross(observed_point, observed_axis) + residuals.extend(float(value) for value in predicted_axis - observed_axis) + residuals.extend( + float(value) / 0.05 + for value in predicted_moment - observed_moment + ) + return np.asarray(residuals, dtype=float) + + observability_parameters = np.asarray( + [ + *base_rotation.as_rotvec(), + *(base_translation / 0.05), + *( + training_offsets[name] + for name in profile.direct_zero_joints + ), + ], + dtype=float, + ) + observability_base_residual = observability_residual( + observability_parameters + ) + observability_jacobian = np.empty( + ( + observability_base_residual.size, + observability_parameters.size, + ), + dtype=float, + ) + finite_difference_step = 1.0e-6 + for column in range(observability_parameters.size): + positive = observability_parameters.copy() + negative = observability_parameters.copy() + positive[column] += finite_difference_step + negative[column] -= finite_difference_step + observability_jacobian[:, column] = ( + observability_residual(positive) + - observability_residual(negative) + ) / (2.0 * finite_difference_step) + singular_values = np.linalg.svd( + observability_jacobian, compute_uv=False + ) + singular_threshold = ( + 0.0 + if singular_values.size == 0 + else float(singular_values[0]) * 1.0e-7 + ) + observability_rank = int( + np.count_nonzero(singular_values > singular_threshold) + ) + observability_parameter_count = int(observability_parameters.size) + observability_condition_number = ( + float("inf") + if observability_rank < observability_parameter_count + else float(singular_values[0] / singular_values[-1]) + ) + residual_dof = max( + 1, + observability_base_residual.size - observability_parameter_count, + ) + residual_variance = float( + observability_base_residual @ observability_base_residual + ) / residual_dof + covariance = residual_variance * np.linalg.pinv( + observability_jacobian.T @ observability_jacobian, + rcond=1.0e-12, + ) + offset_covariance = { + name: max(0.0, float(covariance[6 + index, 6 + index])) + for index, name in enumerate(profile.direct_zero_joints) + } + def zero_observation_failure_reasons( selected: Sequence[JointAxisMeasurement], offsets: Mapping[str, float], @@ -1641,12 +2353,52 @@ def solve_urdf_zero_offsets( else: parent_joint = profile.phase_parent_joint[observer_joint] phase_items: list[JointAxisMeasurement] = [] + propagated_angle_uncertainties: list[float] = [] for item in observer_items: phase_items.append(item) parent_item = by_key.get((parent_joint, item.cycle)) if parent_item is not None: phase_items.append(parent_item) - if any( + parent_axis = np.asarray( + parent_item.axis_common_xyz, dtype=float + ) + parent_axis /= np.linalg.norm(parent_axis) + separation = ( + np.asarray(item.point_common_xyz_m, dtype=float) + - np.asarray( + parent_item.point_common_xyz_m, dtype=float + ) + ) + radial = separation - parent_axis * float( + separation @ parent_axis + ) + effective_distance = float(np.linalg.norm(radial)) + if effective_distance <= 1.0e-6: + propagated_angle_uncertainties.append(float("inf")) + else: + line_uncertainty = math.hypot( + item.pose_axis_line_rms_m, + parent_item.pose_axis_line_rms_m, + ) + propagated_angle_uncertainties.append( + math.atan2( + line_uncertainty, effective_distance + ) + ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + angle_limit = ( + maximum_confidence_half_width_rad + if maximum_confidence_half_width_rad is not None + else math.radians(0.5) + ) + if ( + not propagated_angle_uncertainties + or max(propagated_angle_uncertainties) > angle_limit + ): + failures[offset_joint] = ( + "zero_phase_axis_line_angle_uncertainty_too_large" + ) + elif any( item.pose_axis_line_rms_m > maximum_pose_axis_line_rms_m for item in phase_items @@ -1663,8 +2415,8 @@ def solve_urdf_zero_offsets( name: [] for name in profile.direct_zero_joints } cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} - all_cycles = sorted({int(item.cycle) for item in measurements}) - for cycle in all_cycles: + training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) + for cycle in training_cycle_ids: selected = [item for item in measurements if item.cycle == cycle] # Keep one palm pose while comparing cycles. Refitting a base pose from # only two nearly parallel root axes per cycle makes harmless root-line @@ -1680,6 +2432,7 @@ def solve_urdf_zero_offsets( for name, value in cycle_offsets.items(): cycle_values[name].append(value) uncertainties: dict[str, float] = {} + confidence_half_widths: dict[str, float] = {} cycle_consistent = True inconsistent_cycles: list[str] = [] for name, values in cycle_values.items(): @@ -1693,6 +2446,14 @@ def solve_urdf_zero_offsets( if array.size < 2 else float(np.std(array, ddof=1) / math.sqrt(array.size)) ) + confidence_half_widths[name] = ( + float("inf") + if array.size < 2 + else float( + student_t.ppf(0.975, df=array.size - 1) + * uncertainties[name] + ) + ) insignificant_large: list[str] = [] applied_training: dict[str, float] = {} @@ -1701,9 +2462,11 @@ def solve_urdf_zero_offsets( applied_training[name] = fixed_offsets[name] continue uncertainty = uncertainties[name] + confidence_half_width = confidence_half_widths[name] if ( abs(value) < minimum_applied_offset_rad - or abs(value) <= significance_sigma * uncertainty + or abs(value) + <= max(significance_sigma * uncertainty, confidence_half_width) ): applied_training[name] = 0.0 if abs(value) >= minimum_applied_offset_rad: @@ -1742,7 +2505,7 @@ def solve_urdf_zero_offsets( improvement_confidence_lower[offset_joint] = 0.0 continue cycle_improvements: list[float] = [] - for cycle in all_cycles: + for cycle in training_cycle_ids: selected = [item for item in measurements if item.cycle == cycle] cycle_rotation, cycle_translation = cycle_models[cycle] candidate = abs( @@ -1763,27 +2526,114 @@ def solve_urdf_zero_offsets( ) cycle_improvements.append(original - candidate) improvement_array = np.asarray(cycle_improvements, dtype=float) - bootstrap_indices = generator.integers( - 0, - improvement_array.size, - size=(2000, improvement_array.size), + improvement_se = ( + float("inf") + if improvement_array.size < 2 + else float( + np.std(improvement_array, ddof=1) + / math.sqrt(improvement_array.size) + ) ) - bootstrap_means = np.mean( - improvement_array[bootstrap_indices], axis=1 + lower = ( + float("-inf") + if not math.isfinite(improvement_se) + else float( + np.mean(improvement_array) + - student_t.ppf( + 0.975, df=improvement_array.size - 1 + ) + * improvement_se + ) ) - lower = float(np.percentile(bootstrap_means, 2.5)) improvement_confidence_lower[offset_joint] = lower if lower <= 0.0: improvement_passed = False validation_errors = np.asarray( list(validation_error_by_joint.values()), dtype=float ) - configured_limit_exceeded = [ + validation_line_samples: dict[str, list[float]] = {} + for item in validation: + predicted_axis, predicted_point = predicted_local( + item, applied_training + ) + predicted_axis = base_rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = ( + base_rotation.apply(predicted_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + cross = np.cross(predicted_axis, observed_axis) + cross_norm = float(np.linalg.norm(cross)) + separation = observed_point - predicted_point + line_error = ( + abs(float(separation @ cross)) / cross_norm + if cross_norm > 1.0e-6 + else float( + np.linalg.norm( + separation + - predicted_axis * float(separation @ predicted_axis) + ) + ) + ) + validation_line_samples.setdefault(item.joint, []).append(line_error) + validation_line_error_by_joint = { + name: float(np.sqrt(np.mean(np.square(values)))) + for name, values in validation_line_samples.items() + } + all_validation_line_errors = np.asarray( + [ + value + for values in validation_line_samples.values() + for value in values + ], + dtype=float, + ) + axis_line_rms = ( + float("inf") + if all_validation_line_errors.size == 0 + else float( + np.sqrt(np.mean(np.square(all_validation_line_errors))) + ) + ) + product_finger_rolls = tuple( name - for name, limit in zip(profile.direct_zero_joints, offset_limits) - if name not in fixed_offsets - and abs(training_offsets[name]) > limit + math.radians(0.01) - ] + for name in profile.direct_zero_joints + if name.endswith("_mcp_roll") and not name.startswith("thumb_") + ) + finger_roll_common_mode = ( + float( + np.median( + [training_offsets[name] for name in product_finger_rolls] + ) + ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and product_finger_rolls + else 0.0 + ) + configured_limit_exceeded: list[str] = [] + for name, limit in zip(profile.direct_zero_joints, offset_limits): + if name in fixed_offsets: + continue + checked_offset = training_offsets[name] + if name in product_finger_rolls: + # The four roll motors share the same electrical centre and the + # absolute palm axial datum is recovered from the root-line + # pattern. Protect the independently assembled finger-to-finger + # deviations with the strict finger bound; protect their shared + # common mode with the unchanged global zero bound. Treating the + # same common datum as four independent failures is both + # over-counting and sensitive to the palm-frame gauge. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) > limit + math.radians(0.01): + configured_limit_exceeded.append(name) + if ( + product_finger_rolls + and abs(finger_roll_common_mode) + > maximum_offset_rad + math.radians(0.01) + ): + configured_limit_exceeded.append("finger_mcp_roll_common_mode") diagnostic_bound_hits = [ name for name, limit in zip( @@ -1793,6 +2643,21 @@ def solve_urdf_zero_offsets( and abs(training_offsets[name]) >= limit - math.radians(0.01) ] failure_reasons: dict[str, str] = {} + if ( + profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and observability_rank < observability_parameter_count + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_rank_deficient" + ) + elif ( + profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and observability_condition_number + > maximum_observability_condition_number + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_ill_conditioned" + ) for name in configured_limit_exceeded: failure_reasons[name] = "zero_offset_exceeds_configured_limit" for name in diagnostic_bound_hits: @@ -1801,6 +2666,19 @@ def solve_urdf_zero_offsets( failure_reasons[name] = "zero_offset_cycle_difference_too_large" for name in insignificant_large: failure_reasons[name] = "zero_offset_not_statistically_significant" + if maximum_confidence_half_width_rad is not None: + for name, half_width in confidence_half_widths.items(): + if half_width > maximum_confidence_half_width_rad: + failure_reasons[name] = ( + "zero_offset_confidence_interval_too_wide" + ) + # This solver publishes rotational encoder zeros only. A post-fit CAD to + # measured axis-line displacement is invariant to the joint's own zero + # and cannot be repaired by changing that rotational parameter. Keep the + # per-joint and aggregate values in ZeroSolveResult for geometry audit, + # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a + # failed rotational holdout. Axis-point *fit* quality is still guarded + # above for every phase observation that actually uses line position. if not improvement_passed: for name, value in applied_training.items(): if value != 0.0 and improvement_confidence_lower.get(name, 0.0) <= 0.0: @@ -1814,36 +2692,51 @@ def solve_urdf_zero_offsets( and float(np.mean(validation_errors)) <= maximum_validation_mae_rad and float(np.percentile(validation_errors, 95.0)) <= maximum_validation_p95_rad + and ( + maximum_validation_error_rad is None + or float(np.max(validation_errors)) + <= maximum_validation_error_rad + ) and cycle_consistent and not insignificant_large and not configured_limit_exceeded and not diagnostic_bound_hits and not observation_failures + and ( + profile.hand.layout_id != G20_RIGHT_19_LAYOUT + or ( + observability_rank == observability_parameter_count + and observability_condition_number + <= maximum_observability_condition_number + ) + ) + and not any( + reason == "zero_offset_confidence_interval_too_wide" + for reason in failure_reasons.values() + ) and improvement_passed ) + # Never refit a model that has passed its holdout with the validation + # cycle. The published offsets are exactly the frozen training result + # that produced ``validation_errors`` above. final_offsets = dict(applied_training) - if passed: - base_rotation, base_translation, all_offsets_fit = solve_selected( - list(measurements), initial=training_offsets - ) - final_offsets = { - name: ( - fixed_offsets[name] - if name in fixed_offsets - else 0.0 - if ( - abs(value) < minimum_applied_offset_rad - or abs(value) <= significance_sigma * uncertainties[name] - ) - else value - ) - for name, value in all_offsets_fit.items() - } # Apply independently validated assembly datums only after trajectory # fitting and holdout validation. A mechanical prior must define the # written artifact without perturbing downstream yaw/pitch estimates. final_offsets.update(output_offsets) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT and product_finger_rolls: + # The camera solve observes the four roll axes in a fitted palm frame. + # Rotation of that frame about their shared datum is a gauge, not four + # independent finger assembly errors. The product command 127/CAD + # pose defines the common straight-ahead datum; publish only each + # finger's robust deviation from the four-finger median. Validation + # above remains in the observation gauge, so no measured residual is + # discarded. + for name in product_finger_rolls: + final_offsets[name] = ( + float(final_offsets[name]) - finger_roll_common_mode + ) # Every active joint must be present in the runtime payload/URDF writer, # but absence of an absolute observation is not evidence for the @@ -1862,23 +2755,104 @@ def solve_urdf_zero_offsets( base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), validation_errors_rad=tuple(float(value) for value in validation_errors), validation_error_by_joint_rad=validation_error_by_joint, - validation_line_error_by_joint_m={}, - axis_line_rms_m=0.0, + validation_line_error_by_joint_m=validation_line_error_by_joint, + axis_line_rms_m=axis_line_rms, passed=passed, cycle_offsets_rad={ name: tuple(float(value) for value in values) for name, values in cycle_values.items() }, offset_uncertainty_rad=uncertainties, + offset_confidence_half_width_rad=confidence_half_widths, + training_cycles=training_cycle_ids, + validation_cycle=int(validation_cycle), validation_original_error_by_joint_rad=original_error_by_joint, validation_improvement_by_joint_rad=improvement_by_joint, validation_improvement_confidence_lower_rad=( improvement_confidence_lower ), + observability_rank=observability_rank, + observability_parameter_count=observability_parameter_count, + observability_condition_number=observability_condition_number, + offset_covariance_rad2=offset_covariance, failure_reasons=failure_reasons, ) +def _files_have_identical_contents(left: Path, right: Path) -> bool: + if left.stat().st_size != right.stat().st_size: + return False + with left.open("rb") as left_stream, right.open("rb") as right_stream: + while True: + left_chunk = left_stream.read(1024 * 1024) + right_chunk = right_stream.read(1024 * 1024) + if left_chunk != right_chunk: + return False + if not left_chunk: + return True + + +def _materialize_relative_mesh_assets( + *, source: Path, output: Path, urdf_root: ET.Element +) -> tuple[Path, ...]: + """Copy relative mesh resources so a session-local URDF remains loadable.""" + filenames = sorted( + { + str(mesh.get("filename", "")).strip() + for mesh in urdf_root.findall(".//mesh") + if str(mesh.get("filename", "")).strip() + } + ) + materialized: list[Path] = [] + for filename in filenames: + # URI-backed resources are resolved by the URDF consumer. Only local + # relative resources need to follow a URDF copied to a session folder. + if "://" in filename or filename.startswith("package:"): + continue + relative = Path(filename) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError( + f"URDF mesh path must be a safe relative path or URI: {filename}" + ) + source_asset = (source.parent / relative).resolve() + if not source_asset.is_file(): + raise ValueError(f"URDF mesh resource does not exist: {source_asset}") + destination_asset = (output / relative).resolve() + try: + destination_asset.relative_to(output) + except ValueError as error: + raise ValueError( + f"URDF mesh destination escapes output directory: {filename}" + ) from error + if destination_asset == source_asset: + materialized.append(destination_asset) + continue + destination_asset.parent.mkdir(parents=True, exist_ok=True) + if destination_asset.exists(): + if not destination_asset.is_file() or not _files_have_identical_contents( + source_asset, destination_asset + ): + raise ValueError( + f"refusing to overwrite a different mesh resource: " + f"{destination_asset}" + ) + materialized.append(destination_asset) + continue + temporary_asset = destination_asset.with_name( + f".{destination_asset.name}.{os.getpid()}.tmp" + ) + if temporary_asset.exists(): + raise ValueError(f"temporary mesh path is occupied: {temporary_asset}") + try: + shutil.copy2(source_asset, temporary_asset) + os.replace(temporary_asset, destination_asset) + finally: + if temporary_asset.exists(): + temporary_asset.unlink() + materialized.append(destination_asset) + return tuple(materialized) + + def write_zero_corrected_urdf( *, source_urdf: str | Path, @@ -1928,6 +2902,12 @@ def write_zero_corrected_urdf( name = str(joint.get("name")) if name not in offsets: continue + found.add(name) + # A statistically insignificant correction is represented as exact + # zero. Preserve that joint's source text byte-for-byte instead of + # serialising an equivalent Euler triplet. + if abs(offsets[name]) <= 1.0e-15: + continue if joint.get("type") not in {"revolute", "continuous"}: raise ValueError(f"joint {name} is not revolute") axis_node = joint.find("axis") @@ -1952,7 +2932,6 @@ def write_zero_corrected_urdf( replacement_rpy[name] = " ".join( f"{float(value):.15g}" for value in corrected_rpy ) - found.add(name) missing = sorted(set(offsets) - found) if missing: raise ValueError("source URDF is missing target joints: " + ",".join(missing)) @@ -1983,6 +2962,7 @@ def write_zero_corrected_urdf( corrected_text = original_text for start, end, value in reversed(edits): corrected_text = corrected_text[:start] + value + corrected_text[end:] + _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) temporary = destination.with_suffix(".urdf.tmp") with temporary.open("w", encoding="utf-8") as stream: stream.write(corrected_text) diff --git a/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py index 25a4a46..810b1c5 100644 --- a/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py @@ -1,4 +1,4 @@ -"""Publish calibrated G20 URDF joint angles from raw u8 commands.""" +"""Publish calibrated G20 URDF angles from raw command/feedback u8 values.""" from launch import LaunchDescription from launch.actions import DeclareLaunchArgument 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 515a176..25776a9 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 @@ -3,6 +3,7 @@ from __future__ import annotations from datetime import datetime +import hashlib from pathlib import Path import re @@ -41,6 +42,28 @@ def _launch_stack(context): hand_type = LaunchConfiguration("hand_type").perform(context).lower() if hand_type not in {"left", "right"}: raise RuntimeError("hand_type must be left or right") + tag_layout = LaunchConfiguration("tag_layout").perform(context).lower() + if tag_layout not in {"legacy_11", "g20_right_15"}: + raise RuntimeError("tag_layout must be legacy_11 or g20_right_15") + if tag_layout == "g20_right_15" and hand_type != "right": + raise RuntimeError("g20_right_15 requires hand_type:=right") + requested_tag_config = LaunchConfiguration("tag_config").perform(context) + package_share = Path( + get_package_share_directory("g20_thumb_apriltag_calibration") + ) + tag_config = ( + Path(requested_tag_config).expanduser().resolve() + if requested_tag_config + else package_share + / "config" + / ( + "three_camera_tags_g20_right_15.yaml" + if tag_layout == "g20_right_15" + else "three_camera_tags.yaml" + ) + ) + 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" @@ -52,6 +75,20 @@ def _launch_stack(context): ) if not source_urdf.is_file(): raise RuntimeError(f"source URDF does not exist: {source_urdf}") + expected_source_hash = LaunchConfiguration( + "source_urdf_expected_sha256" + ).perform(context).strip().lower() + if tag_layout == "g20_right_15": + if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: + raise RuntimeError( + "g20_right_15 requires source_urdf_expected_sha256 confirmed " + "by the CAD/hardware owner" + ) + actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest() + if actual_source_hash != expected_source_hash: + raise RuntimeError( + "source_urdf_expected_sha256 does not match source_urdf_path" + ) hand_serial = LaunchConfiguration("serial_number").perform(context) if ( @@ -163,7 +200,7 @@ def _launch_stack(context): name="apriltag", namespace=detector_namespace, parameters=[ - LaunchConfiguration("tag_config"), + str(tag_config), { "detector.decimate": ParameterValue( LaunchConfiguration("apriltag_decimate"), @@ -234,7 +271,11 @@ def _launch_stack(context): { "serial_number": hand_serial, "hand_type": hand_type, + "tag_layout": tag_layout, "session_dir": str(session_dir), + "resume_raw_samples_path": LaunchConfiguration( + "resume_raw_samples_path" + ), # The SDK performs roughly 25 synchronous CAN queries whenever # cb__hand_info has a subscriber. Calibration only used # that topic to display a speed diagnostic, while those reads @@ -246,6 +287,9 @@ def _launch_stack(context): "camera_extrinsics_file" ), "source_urdf_path": str(source_urdf), + "source_urdf_expected_sha256": LaunchConfiguration( + "source_urdf_expected_sha256" + ), "corrected_urdf_output_dir": LaunchConfiguration( "corrected_urdf_output_dir" ), @@ -267,6 +311,13 @@ def _launch_stack(context): LaunchConfiguration("index_flex_calibration_speed"), value_type=int, ), + "adaptive_formal_speed_enabled": ParameterValue( + LaunchConfiguration("adaptive_formal_speed_enabled"), + value_type=bool, + ), + "cross_view_roll_diagnostic_finger": LaunchConfiguration( + "cross_view_roll_diagnostic_finger" + ), "validation_enabled": ParameterValue( LaunchConfiguration("validation_enabled"), value_type=bool ), @@ -300,7 +351,7 @@ def _launch_stack(context): return [ LogInfo( msg=( - f"G20 {hand_type} three-camera session: {session_dir}; " + f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; " f"source_urdf={source_urdf}" ) ), @@ -342,6 +393,7 @@ def generate_launch_description() -> LaunchDescription: value=str(package_share / "config" / "fastdds_large_images.xml"), ), DeclareLaunchArgument("hand_type", default_value="left"), + DeclareLaunchArgument("tag_layout", default_value="legacy_11"), DeclareLaunchArgument("serial_number", default_value="UNSET"), DeclareLaunchArgument( "front_camera_serial", default_value="DB2163742" @@ -387,6 +439,12 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "index_flex_calibration_speed", default_value="10" ), + DeclareLaunchArgument( + "adaptive_formal_speed_enabled", default_value="true" + ), + DeclareLaunchArgument( + "cross_view_roll_diagnostic_finger", default_value="" + ), DeclareLaunchArgument("validation_enabled", default_value="false"), DeclareLaunchArgument( "camera_extrinsics_file", @@ -397,6 +455,9 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "source_urdf_path", default_value="" ), + DeclareLaunchArgument( + "source_urdf_expected_sha256", default_value="" + ), DeclareLaunchArgument( "corrected_urdf_output_dir", default_value="" ), @@ -409,6 +470,7 @@ def generate_launch_description() -> LaunchDescription: default_value=str(Path.cwd() / "calibration_output"), ), DeclareLaunchArgument("session_dir", default_value=""), + DeclareLaunchArgument("resume_raw_samples_path", default_value=""), DeclareLaunchArgument( "calibration_config", default_value=str( @@ -417,9 +479,7 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "tag_config", - default_value=str( - package_share / "config" / "three_camera_tags.yaml" - ), + default_value="", ), OpaqueFunction(function=_launch_stack), ] diff --git a/src/g20_thumb_apriltag_calibration/package.xml b/src/g20_thumb_apriltag_calibration/package.xml index ed71c5b..d3eac51 100644 --- a/src/g20_thumb_apriltag_calibration/package.xml +++ b/src/g20_thumb_apriltag_calibration/package.xml @@ -3,7 +3,7 @@ g20_thumb_apriltag_calibration 0.1.0 - Three-view Hikrobot AprilTag calibration for the complete left G20 hand. + One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right hand. lxp MIT diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index c14c7a9..ca6c069 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="Three-view Hikrobot AprilTag calibration for the complete left G20 hand", + description="One-command three-camera AprilTag calibration for the G20 right hand", license="MIT", entry_points={ "console_scripts": [ @@ -64,6 +64,10 @@ setup( "g20_thumb_apriltag_calibration." "calibrated_joint_state_bridge:main" ), + ( + "calibrate_g20_right = " + "g20_thumb_apriltag_calibration.one_command:main" + ), ], }, ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py index 9b2ead7..bff09a6 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py +++ b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py @@ -6,10 +6,16 @@ from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import ( G20_COMMAND_NAMES, G20_URDF_JOINT_NAMES, CalibratedCommandMapper, + default_input_topic, ) from g20_thumb_apriltag_calibration.full_hand import ( + JointCurveFit, + build_compact_payload, get_hand_calibration_profile, ) +from g20_thumb_apriltag_calibration.urdf_zero import ( + get_zero_calibration_profile, +) def _payload(side: str = "right") -> dict: @@ -85,3 +91,87 @@ def test_mapper_rejects_incomplete_named_command() -> None: mapper = CalibratedCommandMapper(_payload(), expected_side="right") with pytest.raises(ValueError, match="missing named channels"): mapper.map_positions([255.0], ["thumb_cmc_pitch"]) + + +def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None: + profile = get_hand_calibration_profile("right", "g20_right_15") + zero = get_zero_calibration_profile("right", "g20_right_15") + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + fits = {} + diagnostics = {} + for name, spec in profile.joint_specs.items(): + zero_command = baseline[spec.motor_index] + average = tuple( + 0.001 * (zero_command - value) for value in range(256) + ) + decreasing = tuple(1.1 * value for value in average) + increasing = tuple(0.9 * value for value in average) + fits[name] = JointCurveFit( + average, + decreasing, + increasing, + {}, + 0.0, + 0.0, + {"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5}, + ) + diagnostics[name] = { + "cycle_travel_rad": [0.25] * 4, + "cycle_travel_range_rad": 0.0, + "baseline_hysteresis_by_cycle_rad": [0.0] * 4, + "holdout_cycle": 3, + "holdout_cycle_mae_rad": 0.0, + "holdout_cycle_max_rad": 0.0, + } + payload = build_compact_payload( + serial_number="TEST_RIGHT_V5", + measured_fits=fits, + urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints}, + validation_errors_rad=[0.0], + passed=True, + baseline=baseline, + side="right", + layout_id="g20_right_15", + zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints}, + zero_cycle_offsets_rad={ + name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints + }, + zero_observers=zero.offset_observer_joint, + artifact_hashes={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "corrected_urdf_sha256": "2" * 64, + }, + cross_view_roll_metrics={ + f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0} + for finger in ("index", "middle", "ring", "pinky") + }, + joint_dynamic_diagnostics=diagnostics, + zero_geometry_diagnostics={ + "training_cycles": (0, 1, 2), + "validation_cycle": 3, + "axis_line_rms_m": 0.0, + "validation_line_error_by_joint_m": {}, + }, + ) + mapper = CalibratedCommandMapper(payload, expected_side="right") + command = [255.0] * 20 + command[0] = 100.0 + mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))) + + assert mapper.input_domain == "command_u8" + assert mapper.layout_id == "g20_right_15" + # midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve. + assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155) + assert payload["joints"]["pinky_dip"]["passive"] is True + assert "source_joint" not in payload["joints"]["pinky_dip"] + + +def test_default_topics_use_the_g20_sdk_namespace() -> None: + assert default_input_topic("right", "command_u8") == ( + "/g20/cb_right_hand_control_cmd" + ) + assert default_input_topic("right", "feedback_u8") == ( + "/g20/cb_right_hand_state" + ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index f28b4d9..94bb0d1 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -80,6 +80,36 @@ def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None: ]["ros__parameters"]["tag"]["frames"][0] == "side_base" +def test_right_15_tag_config_matches_the_physical_layout() -> None: + tags = yaml.safe_load( + ( + PACKAGE_ROOT + / "config" + / "three_camera_tags_g20_right_15.yaml" + ).read_text() + ) + expected = { + "front": [0, 1, 2, 3, 10, 11, 12, 13], + "side": [4, 5, 6, 15, 17], + "top": [8, 9], + } + all_ids: set[int] = set() + for view, ids in expected.items(): + parameters = tags[ + f"/g20_calibration/{view}/apriltag/apriltag" + ]["ros__parameters"] + assert parameters["tag"]["ids"] == ids + assert parameters["tag"]["sizes"] == [0.016] * len(ids) + all_ids.update(ids) + assert all_ids == set(range(19)) - {7, 14, 16, 18} + side_frames = tags[ + "/g20_calibration/side/apriltag/apriltag" + ]["ros__parameters"]["tag"]["frames"] + assert side_frames == [ + "side_base", "ring_pip", "pinky_pip", "middle_pip", "index_pip", + ] + + def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text() @@ -114,14 +144,35 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["normal_calibration_speed"] == 15 assert parameters["index_roll_calibration_speed"] == 5 assert parameters["index_flex_calibration_speed"] == 10 + assert parameters["adaptive_formal_speed_enabled"] is True + assert parameters["adaptive_formal_speed_max_scale"] == 1.5 + assert parameters["adaptive_formal_speed_minimum_bins"] == 64 + assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8 assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0 assert parameters["speed_setting_settle_seconds"] >= 0.2 assert parameters["top_pnp_invalid_reset_seconds"] == 1.0 assert parameters["pnp_group_initialization_frames"] == 8 assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0 assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0 + assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03 + assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0 + assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5 + assert parameters["baseline_hold_seconds"] == 0.5 + assert parameters["minimum_baseline_hold_frames"] == 10 + assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0 + assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3 + assert ( + parameters["cross_view_roll_maximum_branch_gap_difference_deg"] + == 0.3 + ) + assert parameters["cross_view_roll_diagnostic_finger"] == "" assert parameters["repetitions"] == 3 + assert parameters["g20_right_19_repetitions"] >= 4 assert parameters["validation_enabled"] is False + assert parameters["combination_validation_enabled"] is False + assert parameters["combination_validation_frames"] >= 10 + assert parameters["combination_maximum_position_p95_m"] <= 0.003 + assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0 assert parameters["minimum_detection_rate"] == 0.95 assert parameters["minimum_detection_hz"] == 15.0 assert parameters["minimum_state_span_u8"] >= 240.0 @@ -130,14 +181,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0 assert parameters["minimum_sweep_bins"] >= 32 assert parameters["maximum_bin_gap"] <= 16 - assert parameters["automatic_sweep_retry_limit"] == 3 + assert parameters["automatic_sweep_retry_limit"] == 2 assert parameters["automatic_fit_retry_limit"] == 2 - assert parameters["motor_stall_timeout_seconds"] >= 5.0 + assert parameters["motor_stall_timeout_seconds"] == 2.0 + assert parameters["motor_stall_startup_grace_seconds"] == 1.0 assert parameters["motor_stall_minimum_progress_u8"] == 1.0 assert parameters["automatic_motion_retry_limit"] == 2 assert parameters["provisional_warning_ratio"] == 1.25 - assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5] - assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25] + assert parameters["retry_speed_scales"] == [0.8, 0.6] + assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0] assert parameters["position_timeout_seconds"] >= 20.0 assert parameters["maximum_state_image_skew_ms"] <= 50.0 assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0 @@ -147,6 +199,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5 assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75 assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0 + assert parameters["zero_maximum_observability_condition_number"] >= 1.0 assert parameters["zero_maximum_offset_deg"] <= 20.0 assert parameters["zero_finger_maximum_offset_deg"] <= 3.0 assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0 @@ -155,7 +208,13 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0 assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0 assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0 - assert parameters["passive_maximum_hysteresis_deg"] <= 7.5 + assert parameters["maximum_hysteresis_deg"] <= 2.0 + assert parameters["passive_maximum_hysteresis_deg"] <= 2.0 + assert parameters["command_maximum_direction_gap_deg"] <= 2.0 + assert parameters["maximum_validation_mae_deg"] <= 1.0 + assert parameters["maximum_validation_p95_deg"] <= 2.0 + assert parameters["maximum_validation_error_deg"] <= 3.0 + assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5 for view in ("front", "side", "top"): assert parameters[f"{view}_camera_info_topic"].startswith( f"/g20_calibration/{view}/" 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 84f4808..c7f6fed 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -1,5 +1,6 @@ import math from dataclasses import replace +from pathlib import Path import numpy as np import pytest @@ -8,18 +9,26 @@ from g20_thumb_apriltag_calibration.full_hand import ( ACTIVE_JOINTS, IMAGE_TRAJECTORY_JOINTS, JOINT_SPECS, + JointCurveFit, LEFT_HAND_PROFILE, MEASURED_JOINTS, PASSIVE_JOINTS, RIGHT_HAND_PROFILE, + RIGHT_19_HAND_PROFILE, SPLAY_JOINTS, SWEEP_SPECS, + THREE_CAMERA_BASELINE_COMMAND, VIEW_TAGS, build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, build_calibration_speed_profile, build_compact_payload, build_full_hand_command, + clamp_runtime_fits_to_urdf_limits, center_splay_curve, + compare_cross_view_roll_curves, + derive_mimic_passive_fits, fit_joint_center_curve, fit_joint_image_curve, fit_measured_joint_curve, @@ -30,6 +39,375 @@ from g20_thumb_apriltag_calibration.full_hand import ( ) +def test_runtime_fit_saturates_at_source_urdf_limit() -> None: + values = tuple(np.linspace(0.0, 2.0, 256)) + fit = JointCurveFit( + angle_rad=values, + decreasing_rad=values, + increasing_rad=values, + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.0, + quality={}, + ) + + bounded = clamp_runtime_fits_to_urdf_limits( + Path(__file__).resolve().parents[3] + / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/" + "hands/linker_hand/g20_right/linkerhand_g20_right.urdf", + {"thumb_cmc_yaw": fit}, + )["thumb_cmc_yaw"] + + assert max(bounded.angle_rad) == pytest.approx(1.57) + assert max(bounded.decreasing_rad) == pytest.approx(1.57) + assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0 + + +def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None: + profile = get_hand_calibration_profile("right", "g20_right_15") + assert profile is RIGHT_19_HAND_PROFILE + assert len(profile.sweep_specs) == 16 + assert len(profile.measured_joints) == 17 + assert len(profile.record_joints) == 21 + assert list(profile.view_tags["front"].values()) == [ + 0, 1, 2, 3, 10, 11, 12, 13 + ] + assert list(profile.view_tags["side"].values()) == [ + 4, 5, 6, 15, 17 + ] + assert list(profile.view_tags["top"].values()) == [8, 9] + assert profile.preflight_view_roles == { + "front": ("front_base",), + "side": ("side_base",), + "top": ("top_base",), + } + assert [spec.task_name for spec in profile.sweep_specs[:4]] == [ + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + "thumb_mcp_ip_front", + "thumb_cmc_yaw_top", + ] + assert [ + spec.task_name for spec in profile.sweep_specs[4:7] + ] == [ + "pinky_roll_multiview", + "pinky_pitch_side", + "pinky_pip_side", + ] + assert profile.sweep_specs[4].joints == ( + "pinky_mcp_roll", + "pinky_mcp_roll_side", + ) + assert { + name: spec.source_joint + for name, spec in profile.joint_specs.items() + if spec.source_joint is not None + } == { + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_dip": "ring_pip", + "pinky_dip": "pinky_pip", + } + + +def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None: + profile = RIGHT_19_HAND_PROFILE + values = tuple(0.002 * (255 - command) for command in range(256)) + fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {}) + measured = {name: fit for name in profile.measured_joints} + source_urdf = ( + Path(__file__).resolve().parents[3] + / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/" + "linker_hand/g20_right/linkerhand_g20_right.urdf" + ) + + runtime = derive_mimic_passive_fits( + source_urdf, measured, profile=profile + ) + + assert set(runtime) == set(profile.joint_specs) + for finger in ("index", "middle", "ring", "pinky"): + pip = runtime[f"{finger}_pip"] + dip = runtime[f"{finger}_dip"] + assert dip.angle_rad == pytest.approx( + tuple(0.89 * value for value in pip.angle_rad) + ) + assert dip.maximum_hysteresis_rad == pytest.approx(0.0089) + assert dip.circle["curve_source"] == f"{finger}_pip" + assert dip.circle["visual_measurement"] is False + + +def _changed_motors( + previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...] +) -> list[int]: + return [ + index + for index, (left, right) in enumerate(zip(previous, waypoint)) + if left != right + ] + + +def _phase_changes( + previous: tuple[int, ...] | list[int], + waypoints: tuple[tuple[int, ...], ...], +) -> list[list[int]]: + phases: list[list[int]] = [] + for waypoint in waypoints: + phases.append(_changed_motors(previous, waypoint)) + previous = waypoint + return phases + + +def test_right_19_side_clearance_waypoints_group_joint_classes() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs + if item.task_name == "index_roll_multiview" + ) + baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=baseline, + profile=profile, + ) + # Side clearance keeps all neighbouring roll axes neutral, flexes the + # neighbour PIPs together, then their pitches, and moves the index target + # last through the cleared swept volume. + assert _phase_changes(baseline, waypoints) == [ + [17, 18, 19], [2, 3, 4], [6], + ] + assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255 + + returns = build_calibration_return_waypoints( + baseline, + current_command=waypoints[-1], + profile=profile, + ) + assert _phase_changes(waypoints[-1], returns) == [ + [6], [2, 3, 4], [17, 18, 19], + ] + assert list(returns[-1]) == baseline + + +def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs + if item.task_name == "index_roll_multiview" + ) + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], + profile=profile, + parallel=False, + ) + previous = tuple([255] * 6 + [127] * 4 + [255] * 10) + changed_motors = [] + for waypoint in waypoints: + changed = _changed_motors(previous, waypoint) + assert len(changed) == 1 + changed_motors.append(changed[0]) + previous = waypoint + # Side clearance keeps all neighbouring roll axes neutral, flexes them out + # of the swept volume, and moves the index target last. + assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6] + assert previous[1] == 255 and previous[16] == 255 + + returns = build_calibration_return_waypoints( + [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], + current_command=previous, + profile=profile, + parallel=False, + ) + for waypoint in returns: + changed = sum(left != right for left, right in zip(previous, waypoint)) + assert changed == 1 + previous = waypoint + + +def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs + if item.task_name == "ring_roll_multiview" + ) + current = tuple(THREE_CAMERA_BASELINE_COMMAND) + + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=current, + profile=profile, + ) + + assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]] + assert all(waypoint[9] == 127 for waypoint in waypoints) + assert waypoints[-1][8] == 255 + assert waypoints[-1][19] == 0 + assert waypoints[-1][4] == 0 + + +def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs + if item.task_name == "middle_roll_multiview" + ) + # Session 20260819_160238 stopped here: the legacy front-only avoidance + # asked neighbouring ring roll (motor 8) for 0 and it saturated at 58. + current = [254] * 20 + current[6:10] = [254, 127, 58, 127] + current[10] = 252 + current[11:15] = [0, 0, 0, 0] + + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=current, + profile=profile, + ) + + # Both differing roll axes (parked motor 6 and the stuck motor 8) move in + # the single roll phase, still never commanded toward 0. + phases = _phase_changes(current, waypoints) + assert phases[0] == [6, 8] + assert waypoints[0][8] == 127 + assert all( + waypoint[motor] != 0 + for waypoint in waypoints + for motor in range(6, 10) + ) + assert waypoints[-1][6:10] == (255, 255, 127, 127) + + +def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs + if item.task_name == "ring_pitch_side" + ) + current = tuple(THREE_CAMERA_BASELINE_COMMAND) + + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=current, + profile=profile, + ) + + assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]] + assert all(waypoint[8] == 127 for waypoint in waypoints) + assert all(waypoint[9] == 127 for waypoint in waypoints) + assert waypoints[-1][9] == 127 + assert waypoints[-1][19] == 0 + assert waypoints[-1][4] == 0 + + +def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = profile.sweep_specs[0] + # This is the real G20 feedback shape: reserved channels 11..14 report zero + # while active endpoint channels commonly settle one or two counts low. + current = [ + 255, 254, 254, 254, 254, 254, 127, 127, 127, 127, + 253, 0, 0, 0, 0, 255, 253, 253, 253, 253, + ] + waypoints = build_calibration_preparation_waypoints( + spec, + 255, + current_command=current, + profile=profile, + ) + assert waypoints + assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints) + controlled = {joint.motor_index for joint in profile.joint_specs.values()} + for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints): + assert set(_changed_motors(previous, waypoint)) <= controlled + + returns = build_calibration_return_waypoints( + list(THREE_CAMERA_BASELINE_COMMAND), + current_command=waypoints[-1], + profile=profile, + ) + assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns) + + +def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None: + # Full four-finger avoidance pose: rolls parked at 255 and every clearance + # finger flexed to 0. + current = list(THREE_CAMERA_BASELINE_COMMAND) + current[6:10] = [255, 255, 255, 255] + for motor in (1, 2, 3, 4, 16, 17, 18, 19): + current[motor] = 0 + returns = build_calibration_return_waypoints( + list(THREE_CAMERA_BASELINE_COMMAND), + current_command=current, + profile=RIGHT_19_HAND_PROFILE, + anchor_roll_motor=9, + ) + assert _phase_changes(current, returns) == [ + [6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19], + ] + assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND) + + +def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None: + current = list(THREE_CAMERA_BASELINE_COMMAND) + current[6:10] = [255, 255, 255, 255] + returns = build_calibration_return_waypoints( + list(THREE_CAMERA_BASELINE_COMMAND), + current_command=current, + profile=RIGHT_19_HAND_PROFILE, + anchor_roll_motor=9, + parallel=False, + ) + phases = _phase_changes(current, returns) + assert all(len(phase) == 1 for phase in phases) + assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6] + + +def test_right_19_ring_exit_returns_ring_before_neighbours() -> None: + current = list(THREE_CAMERA_BASELINE_COMMAND) + current[6:10] = [255, 255, 255, 0] + returns = build_calibration_return_waypoints( + list(THREE_CAMERA_BASELINE_COMMAND), + current_command=current, + profile=RIGHT_19_HAND_PROFILE, + anchor_roll_motor=8, + parallel=False, + ) + + first = returns[0] + assert first[8] == 127 + assert first[6:8] == (255, 255) + assert first[9] == 0 + + +def test_cross_view_roll_curve_must_agree_before_fusion() -> None: + values = tuple(0.5 * (127 - command) / 127 for command in range(256)) + fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) + assert compare_cross_view_roll_curves(fit, fit)[ + "angle_rad_rms_difference_rad" + ] == 0.0 + bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values)) + bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values, + increasing_rad=bad_values) + with pytest.raises(ValueError, match="cross_view_roll_curve"): + compare_cross_view_roll_curves(fit, bad) + + reverse = replace( + fit, + increasing_rad=tuple(value + math.radians(0.5) for value in values), + ) + with pytest.raises(ValueError, match="branch_gap_difference"): + compare_cross_view_roll_curves(fit, reverse) + + def _records() -> list[dict[str, object]]: commands = list(range(0, 256, 16)) if commands[-1] != 255: @@ -498,3 +876,88 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None: assert joint["angle_rad"] == source["angle_rad"] assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0} assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01} + + +def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> None: + from g20_thumb_apriltag_calibration.urdf_zero import ( + get_zero_calibration_profile, + ) + + profile = RIGHT_19_HAND_PROFILE + zero = get_zero_calibration_profile("right", "g20_right_15") + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + fits = {} + diagnostics = {} + for name, spec in profile.joint_specs.items(): + zero_command = baseline[spec.motor_index] + values = tuple( + 0.002 * (zero_command - command) for command in range(256) + ) + fits[name] = JointCurveFit( + values, + values, + values, + {}, + 0.0, + 0.0, + {"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5}, + ) + diagnostics[name] = { + "cycle_travel_rad": [0.5, 0.5, 0.5, 0.5], + "cycle_travel_range_rad": 0.0, + "baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0], + "holdout_cycle": 3, + "holdout_cycle_mae_rad": 0.0, + "holdout_cycle_max_rad": 0.0, + } + offsets = {name: 0.0 for name in profile.active_joints} + payload = build_compact_payload( + serial_number="G20_RIGHT_019", + measured_fits=fits, + urdf_zero_offsets_rad=offsets, + validation_errors_rad=[0.0], + passed=True, + baseline=baseline, + side="right", + layout_id="g20_right_15", + zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints}, + zero_cycle_offsets_rad={ + name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints + }, + zero_observers=zero.offset_observer_joint, + artifact_hashes={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "corrected_urdf_sha256": "2" * 64, + }, + cross_view_roll_metrics={ + f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0} + for finger in ("index", "middle", "ring", "pinky") + }, + joint_dynamic_diagnostics=diagnostics, + zero_geometry_diagnostics={ + "training_cycles": (0, 1, 2), + "validation_cycle": 3, + "axis_line_rms_m": 0.0, + "validation_line_error_by_joint_m": {}, + }, + ) + + validate_compact_payload(payload) + assert payload["schema_version"] == 4 + assert set(payload) == { + "schema_version", "model", "side", "serial_number", "angle_unit", + "command_range", "baseline_command_u8", "joints", "quality", + } + assert len(zero.direct_zero_joints) == 12 + assert payload["joints"]["thumb_mcp"]["zero_angles"] == { + "urdf_zero_offset_rad": 0.0 + } + assert payload["joints"]["thumb_ip"]["passive"] is True + assert set(payload["joints"]["thumb_ip"]) == { + "motor_index", "angle_rad", "passive" + } + assert all( + "source_joint" not in joint for joint in payload["joints"].values() + ) 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 new file mode 100644 index 0000000..db10681 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py @@ -0,0 +1,1449 @@ +from dataclasses import replace +from collections import deque +import json +import math +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from g20_thumb_apriltag_calibration.core import ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, +) +from g20_thumb_apriltag_calibration.full_hand import ( + G20_COMBINATION_REQUIRED_TARGET_KEYS, + G20_REFERENCE_THUMB_CMC_JOINTS, + G20_RIGHT_19_LAYOUT, + JointCurveFit, + build_compact_payload, + get_hand_calibration_profile, +) +from g20_thumb_apriltag_calibration.operator_report import ( + ProgressEstimator, + build_failure_report, + classify_error, + render_progress_zh, +) +from g20_thumb_apriltag_calibration.one_command import ( + _automatic_resume_candidate, +) +from g20_thumb_apriltag_calibration.product import load_product_config +from g20_thumb_apriltag_calibration.publication import ( + ACTIVE_ZERO_JOINTS, + PASSIVE_JOINTS, + atomic_session_pointer, + build_mujoco_validation_commands, + finalize_session_artifacts, + validate_runtime_curves_against_urdf_limits, + verify_corrected_urdf, + verify_urdf_mesh_resources, +) +from g20_thumb_apriltag_calibration.storage import atomic_write_json +from g20_thumb_apriltag_calibration.three_camera_node import ( + G20ThreeCameraCalibrationNode, + STEADY_COMMAND_CHECKPOINTS, + SweepItem, + _combination_joint_angles, + _combination_motor_directions, + _combination_validation_items, + _model_link_in_observer_base, + _steady_checkpoint_commands, + _unresolved_fit_failure_tasks, + combination_target_coverage, + resumable_completed_task_prefix, +) +from g20_thumb_apriltag_calibration.urdf_zero import ( + UrdfKinematicModel, + get_zero_calibration_profile, + write_zero_corrected_urdf, +) + + +REPO = Path(__file__).resolve().parents[3] +PRODUCT = REPO / "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml" + + +def _config(tmp_path: Path, *, passes: int = 2): + value = load_product_config(PRODUCT, workspace=REPO, check_can=False) + return replace( + value, + output_root=tmp_path / "calibration_output", + required_independent_passes=passes, + ) + + +def _payload(serial: str = "G20_RIGHT_001") -> dict: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + fits = {} + for name, spec in profile.joint_specs.items(): + zero = baseline[spec.motor_index] + decreasing = tuple(0.0011 * (zero - value) for value in range(256)) + increasing = tuple(0.0009 * (zero - value) for value in range(256)) + fits[name] = JointCurveFit( + angle_rad=decreasing, + decreasing_rad=decreasing, + increasing_rad=increasing, + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=math.radians(1.0), + quality={}, + ) + return build_compact_payload( + serial_number=serial, + measured_fits=fits, + urdf_zero_offsets_rad={name: 0.01 for name in profile.active_joints}, + validation_errors_rad=[0.0, math.radians(0.5)], + passed=True, + baseline=baseline, + side="right", + layout_id=G20_RIGHT_19_LAYOUT, + ) + + +def _make_passed_session(config, stamp: str) -> Path: + session = config.session_root / stamp + session.mkdir(parents=True) + payload = _payload(config.serial_number) + atomic_write_json( + session / f"g20_right_{config.serial_number}_calibration.json", payload + ) + write_zero_corrected_urdf( + source_urdf=config.source_urdf, + output_directory=session, + serial_number=config.serial_number, + offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS}, + timestamp=stamp, + ) + (session / "raw_samples.jsonl").write_text('{"kind":"session_start"}\n') + (session / "calibration.log").write_text("complete\n") + return session + + +def _passed_node_status() -> dict: + observation_counts = { + key: 2 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS + } + validation_counts = { + key: 1 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS + } + return { + "state": "COMPLETE", + "combination_validation": { + "enabled": True, + "completed": True, + "completed_poses": 8, + "total_poses": 8, + "position_p95_m": 0.001, + "orientation_p95_rad": math.radians(1.0), + "required_targets": list(G20_COMBINATION_REQUIRED_TARGET_KEYS), + "observation_counts": observation_counts, + "validation_counts": validation_counts, + }, + } + + +def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: + config = load_product_config(PRODUCT, workspace=REPO, check_can=False) + assert config.serial_number == "G20_RIGHT_001" + assert config.required_independent_passes == 1 + assert set(config.cameras) == {"front", "side", "top"} + assert len({camera["serial_number"] for camera in config.cameras.values()}) == 3 + assert config.source_urdf.is_file() + assert config.camera_extrinsics.is_file() + + +def test_first_round_uses_nine_bidirectional_steady_commands() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + spec = profile.sweep_specs[0] + decreasing = _steady_checkpoint_commands( + profile, SweepItem(spec, 0, "decreasing") + ) + increasing = _steady_checkpoint_commands( + profile, SweepItem(spec, 0, "increasing") + ) + assert (255, *decreasing) == STEADY_COMMAND_CHECKPOINTS + assert (0, *increasing) == tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) + assert _steady_checkpoint_commands( + profile, SweepItem(spec, 1, "decreasing") + ) == () + + +def test_combination_coverage_requires_observation_and_validation_per_target() -> None: + assert G20_REFERENCE_THUMB_CMC_JOINTS == { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_cmc_yaw", + } + assert not any( + "thumb" in key for key in G20_COMBINATION_REQUIRED_TARGET_KEYS + ) + incomplete = combination_target_coverage({}, {}) + assert incomplete["coverage_passed"] is False + assert set(incomplete["missing_validation_targets"]) == set( + G20_COMBINATION_REQUIRED_TARGET_KEYS + ) + + complete = combination_target_coverage( + {key: 2 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS}, + {key: 1 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS}, + ) + assert complete["coverage_passed"] is True + + +def _complete_resume_task_rows(profile, spec) -> list[dict]: + rows: list[dict] = [] + for joint in spec.joints: + for cycle in range(4): + for direction in ("decreasing", "increasing"): + rows.extend( + { + "kind": "sample", + "attempt": 1, + "task_name": spec.key, + "joint": joint, + "cycle": cycle, + "direction": direction, + "feedback_u8": command, + } + for command in (*range(32), 255) + ) + for direction in ("decreasing", "increasing"): + rows.extend( + { + "kind": "steady_command_sample", + "attempt": 1, + "task_name": spec.key, + "joint": joint, + "cycle": 0, + "direction": direction, + "requested_command_u8": command, + "feedback_u8": command, + } + for command in STEADY_COMMAND_CHECKPOINTS + ) + return rows + + +def test_resume_reuses_only_a_fully_committed_task_prefix() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + first, second = profile.sweep_specs[:2] + rows = _complete_resume_task_rows(profile, first) + # A fragment of the following task must be discarded as a unit. + rows.append( + { + "kind": "sample", + "attempt": 1, + "task_name": second.key, + "joint": second.joints[0], + "cycle": 0, + "direction": "decreasing", + "feedback_u8": 255, + } + ) + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], + rows, + ) + + assert completed == (first.key,) + assert reusable + assert {row["task_name"] for row in reusable} == {first.key} + + +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] + rows = [ + *(_complete_resume_task_rows(profile, first)), + *(_complete_resume_task_rows(profile, failed)), + *(_complete_resume_task_rows(profile, later)), + { + "kind": "fit_failure", + "task_name": failed.key, + "view": failed.view, + "motor_index": failed.motor_index, + "joints": list(failed.joints), + "attempt": 1, + "failures": [ + { + "joint": failed.joints[0], + "metric": "monotonic_correction_deg", + "actual": 2.1, + "limit": 2.0, + } + ], + }, + ] + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + allow_sparse=True, + ) + + assert completed == (first.key, later.key) + assert {row["task_name"] for row in reusable} == { + first.key, + later.key, + } + + +def test_sparse_resume_revalidates_retired_product_dynamic_hysteresis() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + task = next( + spec for spec in profile.sweep_specs + if spec.key == "thumb_mcp_ip_front" + ) + rows = [ + *_complete_resume_task_rows(profile, task), + { + "kind": "fit_failure", + "task_name": task.key, + "view": task.view, + "motor_index": task.motor_index, + "joints": list(task.joints), + "attempt": 1, + "failures": [ + { + "joint": "thumb_ip", + "metric": "hysteresis_deg", + "actual": 2.2, + "limit": 2.0, + }, + { + "joint": "thumb_mcp", + "metric": "command_direction_gap_deg", + "actual": 2.4, + "limit": 2.0, + }, + ], + }, + ] + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + allow_sparse=True, + ) + + assert completed == (task.key,) + assert {row["task_name"] for row in reusable} == {task.key} + + +def test_resume_migrates_completed_legacy_split_roll_without_rescanning() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + merged = next( + spec for spec in profile.sweep_specs + if spec.key == "pinky_roll_multiview" + ) + rows = [ + row + for spec in profile.sweep_specs[:4] + for row in _complete_resume_task_rows(profile, spec) + ] + for source in _complete_resume_task_rows(profile, merged): + row = dict(source) + row["task_name"] = ( + "pinky_roll_side" + if row["joint"] == "pinky_mcp_roll_side" + else "pinky_roll_front" + ) + rows.append(row) + for joint in merged.joints: + old_task = ( + "pinky_roll_side" + if joint == "pinky_mcp_roll_side" + else "pinky_roll_front" + ) + for cycle in range(4): + for direction in ("decreasing", "increasing"): + rows.append( + { + "kind": "baseline_hold_sample", + "attempt": 1, + "task_name": old_task, + "joint": joint, + "cycle": cycle, + "direction": direction, + "feedback_u8": 127, + } + ) + + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + + completed, reusable = resumable_completed_task_prefix( + profile, 4, baseline, rows + ) + + assert completed == tuple(spec.key for spec in profile.sweep_specs[:5]) + migrated = [ + row for row in reusable + if row["task_name"] == "pinky_roll_multiview" + ] + assert {row["joint"] for row in migrated} == set(merged.joints) + assert {row["resume_source_task_name"] for row in migrated} == { + "pinky_roll_front", + "pinky_roll_side", + } + + +def test_resume_falls_back_from_an_interrupted_retry_to_complete_attempt() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + pip = next( + spec for spec in profile.sweep_specs + if spec.key == "pinky_pip_side" + ) + pip_index = profile.sweep_specs.index(pip) + rows = [ + row + for spec in profile.sweep_specs[: pip_index + 1] + for row in _complete_resume_task_rows(profile, spec) + ] + # The operator stopped the unnecessary second retry after only two of the + # nine checkpoints. Those partial rows must not hide attempt 1. + for joint in pip.joints: + for command in (255, 224): + rows.append( + { + "kind": "steady_command_sample", + "attempt": 2, + "task_name": pip.key, + "joint": joint, + "cycle": 0, + "direction": "decreasing", + "requested_command_u8": command, + "feedback_u8": command, + } + ) + rows.append( + { + "kind": "fit_failure", + "task_name": pip.key, + "view": pip.view, + "motor_index": pip.motor_index, + "joints": list(pip.joints), + "attempt": 1, + "failures": [ + { + "joint": "pinky_pip", + "metric": "rotation_orthogonal_rms_deg", + "actual": 8.176357, + "limit": 2.5, + } + ], + } + ) + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + ) + + assert completed == tuple( + spec.key for spec in profile.sweep_specs[: pip_index + 1] + ) + checkpoints = [ + row + for row in reusable + if row["kind"] == "steady_command_sample" + and row["direction"] == "decreasing" + ] + assert checkpoints + assert {int(row.get("attempt", 1)) for row in checkpoints} == {1} + + +def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + first = profile.sweep_specs[0] + unresolved_rows = _complete_resume_task_rows(profile, first) + unresolved_rows.append( + { + "kind": "fit_failure", + "task_name": first.key, + "view": first.view, + "motor_index": first.motor_index, + "joints": list(first.joints), + "attempt": 1, + "failures": [ + { + "joint": first.joints[0], + "metric": "arc_deg", + "actual": 10.0, + "limit": 15.0, + } + ], + } + ) + completed, _ = resumable_completed_task_prefix( + profile, 4, [255] * 20, unresolved_rows + ) + assert completed == () + + pitch = next( + spec for spec in profile.sweep_specs if spec.key == "pinky_pitch_side" + ) + retired_rows = [ + { + "kind": "sample", + "task_name": pitch.key, + "attempt": 3, + }, + { + "kind": "fit_failure", + "view": pitch.view, + "motor_index": pitch.motor_index, + "joints": list(pitch.joints), + "attempt": 3, + "failures": [ + { + "joint": "pinky_mcp_pitch", + "metric": "rotation_circle_axis_difference_deg", + "actual": 4.0, + "limit": 1.0, + } + ], + }, + ] + assert _unresolved_fit_failure_tasks(profile, retired_rows) == set() + + +def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None: + config = _config(tmp_path) + session = config.session_root / "20260819_150000" + session.mkdir(parents=True) + (session / "raw_samples.jsonl").write_text( + json.dumps( + { + "kind": "session_start", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "source_urdf_sha256": config.source_urdf_sha256, + } + ) + + "\n" + ) + atomic_write_json( + session / "calibration_summary_zh.json", + { + "result": "FAIL", + "hashes": { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + }, + }, + ) + atomic_session_pointer(config.session_root, "latest_attempt", session) + + assert _automatic_resume_candidate(config) == session + + summary = json.loads( + (session / "calibration_summary_zh.json").read_text() + ) + summary["hashes"]["camera_extrinsics_sha256"] = "0" * 64 + atomic_write_json(session / "calibration_summary_zh.json", summary) + assert _automatic_resume_candidate(config) is None + + +def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary( + tmp_path: Path, +) -> None: + config = _config(tmp_path) + session = config.session_root / "20260819_153000" + session.mkdir(parents=True) + (session / "raw_samples.jsonl").write_text( + json.dumps( + { + "kind": "session_start", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_file": str(config.camera_extrinsics), + } + ) + + "\n", + encoding="utf-8", + ) + atomic_session_pointer(config.session_root, "latest_attempt", session) + + assert not (session / "calibration_summary_zh.json").exists() + assert _automatic_resume_candidate(config) == session + + +def test_automatic_resume_skips_newer_attempt_without_start_checkpoint( + tmp_path: Path, +) -> None: + config = _config(tmp_path) + usable = config.session_root / "20260819_150000" + usable.mkdir(parents=True) + (usable / "raw_samples.jsonl").write_text( + json.dumps( + { + "kind": "session_start", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "source_urdf_sha256": config.source_urdf_sha256, + } + ) + + "\n", + encoding="utf-8", + ) + summary = { + "result": "FAIL", + "hashes": { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + }, + } + atomic_write_json(usable / "calibration_summary_zh.json", summary) + + interrupted = config.session_root / "20260819_160000" + interrupted.mkdir() + (interrupted / "raw_samples.jsonl").write_text("", encoding="utf-8") + atomic_write_json(interrupted / "calibration_summary_zh.json", summary) + atomic_session_pointer(config.session_root, "latest_attempt", interrupted) + + assert _automatic_resume_candidate(config) == usable + + +def test_node_restores_complete_prefix_into_new_self_contained_raw( + tmp_path: Path, monkeypatch +) -> None: + # The synthetic rows carry no pose trajectories, so the import-time hard + # gate revalidation (covered separately in test_three_camera_retry) is + # bypassed here to test the restore plumbing itself. + monkeypatch.setattr( + G20ThreeCameraCalibrationNode, + "_revalidate_imported_tasks", + lambda self, completed, **_kwargs: (tuple(completed), []), + ) + config = _config(tmp_path) + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + source_raw = tmp_path / "old" / "raw_samples.jsonl" + source_raw.parent.mkdir() + rows = [ + { + "kind": "session_start", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "view_tags": { + view: dict(tags) for view, tags in profile.view_tags.items() + }, + "baseline_command_u8": baseline, + "source_urdf_sha256": config.source_urdf_sha256, + }, + *_complete_resume_task_rows(profile, profile.sweep_specs[0]), + ] + source_raw.write_text( + "".join(json.dumps(row) + "\n" for row in rows) + ) + current_raw = tmp_path / "new" / "raw_samples.jsonl" + current_raw.parent.mkdir() + current_raw.touch() + sweep_items = [] + for spec in profile.sweep_specs: + sweep_items.extend( + SweepItem(spec, -1, direction, precheck=True) + for direction in ("decreasing", "increasing") + ) + sweep_items.extend( + SweepItem(spec, cycle, direction) + for cycle in range(4) + for direction in ("decreasing", "increasing") + ) + node = SimpleNamespace( + resume_raw_samples_path=source_raw, + raw_path=current_raw, + hand_type="right", + profile=profile, + baseline_command=tuple(baseline), + source_urdf_path=config.source_urdf, + repetitions=4, + minimum_sweep_bins=32, + records_by_joint={name: [] for name in profile.record_joints}, + baseline_records_by_joint={name: [] for name in profile.record_joints}, + command_records_by_joint={name: [] for name in profile.record_joints}, + sweep_index=0, + sweep_items=sweep_items, + resumed_task_keys=(), + resume_source_session="", + ) + + count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( + node + ) + + assert count == 1 + assert node.sweep_index == 10 + assert node.resumed_task_keys == (profile.sweep_specs[0].key,) + restored = [ + json.loads(line) for line in current_raw.read_text().splitlines() + ] + assert restored[0]["kind"] == "resume_checkpoint_import" + assert any(row["kind"] == "sample" for row in restored) + + +def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> None: + payload = _payload() + assert payload["schema_version"] == 4 + assert len(payload["joints"]) == 21 + assert all("source_joint" not in joint for joint in payload["joints"].values()) + assert sum("zero_angles" in joint for joint in payload["joints"].values()) == 16 + assert { + name for name, joint in payload["joints"].items() if joint.get("passive") + } == PASSIVE_JOINTS + # midpoint of 0.0011 and 0.0009 rad/u8 + assert payload["joints"]["thumb_cmc_pitch"]["angle_rad"][100] == pytest.approx( + 0.155 + ) + + +def test_publication_protects_passive_xml_and_requires_two_matching_sessions( + tmp_path: Path, +) -> None: + config = _config(tmp_path) + first = _make_passed_session(config, "20260819_100000") + first_summary, first_ready = finalize_session_artifacts( + config, first, node_status=_passed_node_status() + ) + assert first_ready is False + assert first_summary["result"] == "PASS_AWAITING_SECOND_SESSION" + assert not (config.session_root / "latest_passed").exists() + + second = _make_passed_session(config, "20260819_110000") + second_summary, second_ready = finalize_session_artifacts( + config, second, node_status=_passed_node_status() + ) + assert second_ready is True + assert second_summary["formal_release"]["comparison_session"] == first.name + assert (config.session_root / "latest_passed").resolve() == second + paths = list(second.glob("*.urdf")) + assert len(paths) == 1 + changed = verify_corrected_urdf(config.source_urdf, paths[0]) + assert set(changed) == ACTIVE_ZERO_JOINTS + resources = verify_urdf_mesh_resources(paths[0]) + assert len(resources) == 22 + assert set(second_summary["hashes"]["mesh_resources_sha256"]) == set( + resources + ) + + commands = json.loads((second / "mujoco_validation_commands.json").read_text()) + assert commands["topic"] == "/g20/cb_right_hand_control_cmd" + assert len(commands["poses"]) == 8 + + +def test_publication_numerically_binds_json_offsets_and_urdf_limits( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + session = _make_passed_session(config, "20260819_111000") + urdf = next(session.glob("*.urdf")) + offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} + + verify_corrected_urdf( + config.source_urdf, urdf, expected_offsets_rad=offsets + ) + wrong_offsets = dict(offsets) + wrong_offsets["thumb_mcp"] = 0.02 + with pytest.raises(ValueError, match="published zero offsets"): + verify_corrected_urdf( + config.source_urdf, + urdf, + expected_offsets_rad=wrong_offsets, + ) + + 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"): + validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) + + +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") + urdf = next(session.glob("*.urdf")) + text = urdf.read_text(encoding="utf-8") + urdf.write_text( + text.replace("hand_base_link", "tampered_base_link", 1), + encoding="utf-8", + ) + + with pytest.raises(ValueError, match="outside active origin.rpy"): + verify_corrected_urdf(config.source_urdf, urdf) + + +def test_publication_requires_every_combination_target(tmp_path: Path) -> None: + config = _config(tmp_path, passes=1) + session = _make_passed_session(config, "20260819_113000") + status = _passed_node_status() + missing = G20_COMBINATION_REQUIRED_TARGET_KEYS[0] + status["combination_validation"]["validation_counts"][missing] = 0 + + with pytest.raises(ValueError, match="coverage is incomplete"): + finalize_session_artifacts(config, session, node_status=status) + + +def test_publication_accepts_formal_holdout_when_combination_diagnostic_disabled( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + session = _make_passed_session(config, "20260819_113100") + status = _passed_node_status() + status["combination_validation"] = { + "enabled": False, + "completed": False, + "completed_poses": 0, + "total_poses": 0, + } + + _summary, release_ready = finalize_session_artifacts( + config, session, node_status=status + ) + + assert release_ready is True + + +def test_publication_accepts_half_lsb_schema_zero_quantisation( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + session = _make_passed_session(config, "20260819_113200") + json_path = session / f"g20_right_{config.serial_number}_calibration.json" + payload = json.loads(json_path.read_text(encoding="utf-8")) + payload["joints"]["thumb_cmc_pitch"]["zero_angles"][ + "urdf_zero_offset_rad" + ] = 0.01 - 4.9e-9 + atomic_write_json(json_path, payload) + + _summary, release_ready = finalize_session_artifacts( + config, session, node_status=_passed_node_status() + ) + + assert release_ready is True + + +def test_publication_saturates_legacy_runtime_curve_before_release( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + session = _make_passed_session(config, "20260819_113300") + json_path = session / f"g20_right_{config.serial_number}_calibration.json" + payload = json.loads(json_path.read_text(encoding="utf-8")) + payload["joints"]["thumb_cmc_yaw"]["angle_rad"][0] = 2.0 + atomic_write_json(json_path, payload) + + summary, release_ready = finalize_session_artifacts( + config, session, node_status=_passed_node_status() + ) + published = json.loads(json_path.read_text(encoding="utf-8")) + + assert release_ready is True + assert published["joints"]["thumb_cmc_yaw"]["angle_rad"][0] == 1.57 + assert summary["runtime_limit_clipped_bins"]["thumb_cmc_yaw"] == 1 + + +def test_latest_attempt_pointer_is_atomic_session_binding(tmp_path: Path) -> None: + root = tmp_path / "G20_RIGHT_001" + first = root / "20260819_120000" + second = root / "20260819_130000" + first.mkdir(parents=True) + second.mkdir() + atomic_session_pointer(root, "latest_attempt", first) + assert (root / "latest_attempt").resolve() == first + atomic_session_pointer(root, "latest_attempt", second) + assert (root / "latest_attempt").resolve() == second + + +def test_failure_report_is_copyable_and_persisted(tmp_path: Path) -> None: + config = _config(tmp_path, passes=1) + session = config.session_root / "20260819_140000" + session.mkdir(parents=True) + status = { + "state": "PAUSED", + "reason": "synchronised_tag_state_timeout", + "feedback_hz": 29.8, + "active": {"joints": ["index_pip"], "valid_frames": 41}, + "views": { + "front": {"ready": True, "missing_tag_ids": []}, + "side": {"ready": False, "missing_tag_ids": [17]}, + "top": {"ready": True, "missing_tag_ids": []}, + }, + } + payload, block = build_failure_report( + config, session, status, reason=status["reason"] + ) + assert payload["error_code"] == "OBS-TAG-103" + assert "请复制以下内容给开发者" in block + assert "会话编号：G20_RIGHT_001_20260819_140000" in block + saved = json.loads((session / "calibration_summary_zh.json").read_text()) + assert saved["quality"]["passed"] is False + + +def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Path) -> None: + config = _config(tmp_path, passes=1) + session = config.session_root / "20260820_172334" + session.mkdir(parents=True) + status = { + "state": "PAUSED", + "reason": "synchronised_tag_state_timeout:front", + "feedback_hz": 58.8, + "active": {"valid_frames": 381}, + "views": { + "front": { + "ready": True, + "missing_tag_ids": [], + "group_pnp_reason": "group_coupled_rotation", + }, + "side": {"ready": True, "missing_tag_ids": []}, + "top": {"ready": True, "missing_tag_ids": []}, + }, + } + + payload, _block = build_failure_report( + config, session, status, reason=status["reason"] + ) + + assert payload["error_code"] == "CAM-GEOMETRY-201" + assert "整组PnP几何检查拒绝" in payload["explanation_zh"] + assert "不要调整" in payload["automatic_action_zh"] + + +def test_multiview_sample_failure_has_stable_observation_code() -> None: + code, problem, _suggestion = classify_error( + "sweep_bins_too_few:pinky_mcp_roll_side", {} + ) + + assert code == "OBS-SAMPLE-104" + assert "轨迹不完整" in problem + + + + +def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: + status = { + "state": "SWEEP", + "progress": 0.43, + "feedback_hz": 98.0, + "resume": { + "used": True, + "completed_task_count": 3, + "total_task_count": 16, + }, + "active": { + "joints": ["index_pip"], + "cycle": 2, + "repetitions": 4, + "direction": "increasing", + "current_motion_target_u8": 117, + "actual_u8": 116.5, + "valid_frames": 80, + "automatic_retry_count": 1, + "fit_attempt": 3, + "fit_attempt_limit": 3, + }, + "views": { + view: { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [0], + "detected_tag_ids": [0], + } + for view in ("front", "side", "top") + }, + } + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + assert "43.0%" in text + assert "食指PIP" in text + assert "相机：3/3" in text + assert "反馈：98.0 Hz" in text + assert "当前任务ID：正面[0✓] 侧面[0✓] 顶部[0✓]" in text + assert "断点：已恢复 3/16 个完整任务" in text + assert "拟合：整关节第 3/3 次尝试" in text + + +def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None: + status = { + "state": "PREFLIGHT", + "progress": 0.0, + "feedback_hz": 40.5, + "active": {}, + "views": { + "front": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "configured_tag_ids": list(range(8)), + "visible_configured_tag_ids": list(range(6)), + "required_tag_ids": [0], + "detected_tag_ids": [0], + }, + "side": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "configured_tag_ids": list(range(9)), + "visible_configured_tag_ids": list(range(5)), + "required_tag_ids": [4], + "detected_tag_ids": [4], + }, + "top": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "configured_tag_ids": [8, 9], + "visible_configured_tag_ids": [8, 9], + "required_tag_ids": [8], + "detected_tag_ids": [8], + }, + }, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "当前 13/19（允许遮挡）" in text + assert "本阶段必需 3/3" in text + + +def test_sweep_progress_lists_visible_and_missing_task_tag_ids() -> None: + status = { + "state": "SWEEP", + "progress": 0.5, + "feedback_hz": 58.8, + "active": { + "joints": ["ring_pip", "ring_dip"], + "cycle": 1, + "repetitions": 4, + "direction": "decreasing", + }, + "views": { + "front": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [0], + "detected_tag_ids": [0], + }, + "side": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [4, 5, 14], + "detected_tag_ids": [4, 14], + }, + "top": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [8], + "detected_tag_ids": [8], + }, + }, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "Tag：4/5 有效" in text + assert "正面[0✓]" in text + assert "侧面[4✓,5✗,14✓]" in text + assert "顶部[8✓]" in text + assert "✓实时可见/锁=基准锁定/✗不可用" in text + + +def test_multiview_progress_marks_occluded_front_base_as_locked() -> None: + status = { + "state": "PREPARE_SWEEP", + "reason": "waiting_for_task_tags_at_sweep_start", + "progress": 0.562, + "feedback_hz": 58.5, + "active": { + "task_name": "middle_roll_multiview", + "joints": ["middle_mcp_roll", "middle_mcp_roll_side"], + "cycle": 0, + "repetitions": 4, + "direction": "decreasing", + "current_motion_target_u8": 255, + "actual_u8": 254.0, + }, + "views": { + "front": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [0, 12], + "locked_reference_tag_ids": [0], + "detected_tag_ids": [12], + }, + "side": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [4, 15], + "locked_reference_tag_ids": [], + "detected_tag_ids": [4, 15], + }, + "top": { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "required_tag_ids": [8], + "locked_reference_tag_ids": [], + "detected_tag_ids": [8], + }, + }, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "已到扫描起点，等待任务Tag" in text + assert "命令/反馈：255/254.0" in text + assert "Tag：5/5 有效（含锁定 1）" in text + assert "正面[0锁,12✓]" in text + assert "预计剩余 等待Tag" in text + + +def test_device_preflight_defers_tag_gate_until_after_baseline_recovery() -> None: + now = 100.0 + state_times = deque( + (now - 0.98 + index * 0.02 for index in range(50)), maxlen=300 + ) + views = { + name: SimpleNamespace( + camera_info_valid=True, + extrinsics_valid=True, + last_message_at=now, + valid_flags=deque([False] * 30, maxlen=30), + detection_times=deque(maxlen=30), + ) + for name in ("front", "side", "top") + } + node = SimpleNamespace( + extrinsics=object(), + latest_state_u8=tuple([127.0] * 20), + last_state_at=now, + state_receive_times=state_times, + minimum_feedback_hz=25.0, + views=views, + ) + + assert G20ThreeCameraCalibrationNode._all_devices_ready(node, now) + + reset_views: list[str] = [] + node._reset_view_trackers = lambda runtime: reset_views.append(runtime.name) + for name, runtime in views.items(): + runtime.name = name + runtime.detection_times.extend([now - 0.1, now]) + G20ThreeCameraCalibrationNode._finish_startup_baseline_recovery(node) + + assert node.state == "PREFLIGHT" + assert node.startup_baseline_recovered is True + assert node.reason == "waiting_for_baseline_tags_after_recovery" + assert reset_views == ["front", "side", "top"] + assert all(not runtime.valid_flags for runtime in views.values()) + assert all(not runtime.detection_times for runtime in views.values()) + + +def test_startup_state_machine_moves_before_applying_tag_gate() -> None: + node = SimpleNamespace( + state="PREFLIGHT", + reason="waiting_for_devices_and_sdk", + started=False, + startup_baseline_recovered=False, + _all_devices_ready=lambda now: True, + _all_preflight_ready=lambda now: (_ for _ in ()).throw( + AssertionError("Tag gate ran before baseline recovery") + ), + ) + + G20ThreeCameraCalibrationNode._advance(node, 10.0) + + assert node.state == "WAIT_START" + assert node.reason == "call_start_for_baseline_recovery" + + node.state = "PREFLIGHT" + node.started = True + node.startup_baseline_recovered = True + node._all_preflight_ready = lambda now: True + started: list[bool] = [] + node._start_next_sweep = lambda: started.append(True) + + G20ThreeCameraCalibrationNode._advance(node, 11.0) + + assert started == [True] + + +def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> None: + status = { + "state": "PREFLIGHT", + "preflight_mode": "device_only_before_baseline", + "progress": 0.0, + "feedback_hz": 58.9, + "active": {}, + "views": { + view: { + "camera_info_valid": True, + "camera_extrinsics_valid": True, + "configured_tag_ids": list(range(6)), + "visible_configured_tag_ids": list(range(3)), + "required_tag_ids": [0], + "detected_tag_ids": [], + } + for view in ("front", "side", "top") + }, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "阶段：设备连接预检" in text + assert "Tag：当前 9/18（基准恢复后检查）" in text + assert "本阶段必需" not in text + + +def test_validation_command_file_has_eight_safe_bounded_poses() -> None: + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + payload = build_mujoco_validation_commands(baseline) + assert len(payload["poses"]) == 8 + assert all( + len(pose["command_u8"]) == 20 + and all(0 <= value <= 255 for value in pose["command_u8"]) + for pose in payload["poses"] + ) + online = _combination_validation_items(baseline) + assert [list(item.command_u8) for item in online] == [ + pose["command_u8"] for pose in payload["poses"] + ] + + +def test_combination_kinematics_can_use_independent_passive_curve() -> None: + config = load_product_config(PRODUCT, workspace=REPO, check_can=False) + model = UrdfKinematicModel(config.source_urdf) + angles = {"thumb_mcp": 0.4, "thumb_ip": 0.2} + independent = model.link_transform( + "thumb_ip", + zero_offsets={}, + joint_angles=angles, + independent_mimic_angles=True, + ) + mimic = model.link_transform( + "thumb_ip", + zero_offsets={}, + joint_angles=angles, + independent_mimic_angles=False, + ) + assert not np.allclose(independent, mimic) + + +def test_combination_angles_follow_baseline_to_target_direction() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + decreasing = tuple(float(value) for value in range(256)) + increasing = tuple(float(value + 1000) for value in range(256)) + average = tuple(float(value + 500) for value in range(256)) + fits = { + name: JointCurveFit( + angle_rad=average, + decreasing_rad=decreasing, + increasing_rad=increasing, + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.0, + quality={}, + ) + for name in profile.joint_specs + } + items = _combination_validation_items(baseline) + directions = _combination_motor_directions(items, 1, baseline) + + angles = _combination_joint_angles( + profile, fits, items[1].command_u8, directions + ) + + assert angles["thumb_cmc_pitch"] == 160.0 + assert angles["index_mcp_roll"] == 127.0 + assert angles["middle_mcp_roll"] == 127.0 + + +def test_combination_directions_include_previous_return_to_baseline() -> None: + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + items = _combination_validation_items(baseline) + + thumb_pose = _combination_motor_directions(items, 1, baseline) + index_pose = _combination_motor_directions(items, 2, baseline) + + assert thumb_pose[0] == DIRECTION_DECREASING + assert thumb_pose[1] == DIRECTION_DECREASING + assert index_pose[0] == DIRECTION_INCREASING + assert index_pose[5] == DIRECTION_INCREASING + assert index_pose[10] == DIRECTION_INCREASING + assert index_pose[15] == DIRECTION_INCREASING + assert index_pose[1] == DIRECTION_DECREASING + assert index_pose[16] == DIRECTION_DECREASING + + +def test_combination_model_link_is_transformed_into_observer_base() -> None: + model_base_common = np.eye(4) + model_base_common[:3, :3] = Rotation.from_euler( + "xyz", [0.4, -0.2, 0.7] + ).as_matrix() + model_base_common[:3, 3] = [0.3, -0.1, 0.8] + observer_mount = np.eye(4) + observer_mount[:3, :3] = Rotation.from_euler( + "xyz", [-0.3, 0.1, 0.2] + ).as_matrix() + observer_mount[:3, 3] = [0.05, 0.02, -0.01] + observer_base_common = model_base_common @ observer_mount + model_link = np.eye(4) + model_link[:3, :3] = Rotation.from_rotvec([0.0, 0.5, 0.0]).as_matrix() + model_link[:3, 3] = [0.08, 0.03, 0.01] + tag_mount = np.eye(4) + tag_mount[:3, 3] = [0.0, 0.0, 0.02] + observed = ( + np.linalg.inv(observer_base_common) + @ model_base_common + @ model_link + @ tag_mount + ) + + resolved = _model_link_in_observer_base( + observer_base_common, model_base_common, model_link + ) + + assert np.allclose(resolved @ tag_mount, observed) + assert not np.allclose(model_link @ tag_mount, observed) + + +def test_node_drops_imported_task_failing_hard_gates( + tmp_path: Path, +) -> None: + config = _config(tmp_path) + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + source_raw = tmp_path / "old_gates" / "raw_samples.jsonl" + source_raw.parent.mkdir() + rows = [ + { + "kind": "session_start", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "view_tags": { + view: dict(tags) for view, tags in profile.view_tags.items() + }, + "baseline_command_u8": baseline, + "source_urdf_sha256": config.source_urdf_sha256, + }, + *_complete_resume_task_rows(profile, profile.sweep_specs[0]), + ] + source_raw.write_text( + "".join(json.dumps(row) + "\n" for row in rows) + ) + current_raw = tmp_path / "new_gates" / "raw_samples.jsonl" + current_raw.parent.mkdir() + current_raw.touch() + sweep_items = [] + for spec in profile.sweep_specs: + sweep_items.extend( + SweepItem(spec, cycle, direction) + for cycle in range(4) + for direction in ("decreasing", "increasing") + ) + node = SimpleNamespace( + resume_raw_samples_path=source_raw, + raw_path=current_raw, + hand_type="right", + profile=profile, + baseline_command=tuple(baseline), + source_urdf_path=config.source_urdf, + repetitions=4, + minimum_sweep_bins=32, + records_by_joint={name: [] for name in profile.record_joints}, + baseline_records_by_joint={ + name: [] for name in profile.record_joints + }, + command_records_by_joint={name: [] for name in profile.record_joints}, + sweep_index=0, + sweep_items=sweep_items, + resumed_task_keys=(), + resume_source_session="", + zero_profile=get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT), + trajectory_maximum_plane_rms_m=0.004, + trajectory_maximum_radial_rms_m=0.004, + trajectory_minimum_radius_m=0.003, + trajectory_minimum_arc_rad=math.radians(15.0), + image_trajectory_maximum_radial_rms_px=2.0, + image_trajectory_maximum_radial_p95_px=3.5, + image_trajectory_minimum_radius_px=20.0, + trajectory_maximum_cycle_travel_difference_rad=math.radians(3.0), + passive_maximum_cycle_travel_difference_rad=math.radians(10.0), + maximum_monotonic_correction_rad=math.radians(2.0), + maximum_hysteresis_rad=math.radians(5.0), + baseline_maximum_hysteresis_rad=math.radians(0.5), + passive_maximum_monotonic_correction_rad=math.radians(3.0), + passive_maximum_hysteresis_rad=math.radians(7.5), + axis_maximum_plane_rms_m=0.003, + passive_axis_maximum_plane_rms_m=0.004, + axis_maximum_radial_rms_m=0.003, + axis_maximum_pose_line_rms_m=0.001, + axis_maximum_rotation_circle_difference_rad=math.radians(1.0), + active_maximum_rotation_orthogonal_rms_rad=math.radians(2.5), + passive_maximum_rotation_orthogonal_rms_rad=math.radians(7.5), + zero_maximum_axis_cycle_difference_rad=math.radians(0.75), + maximum_state_image_skew_ns=50_000_000.0, + minimum_detection_rate=0.95, + ) + node._fit_joint_records = lambda name, records, relaxed=False: ( + G20ThreeCameraCalibrationNode._fit_joint_records( + node, name, records, relaxed=relaxed + ) + ) + node._fit_axis_measurement = lambda name, cycle: ( + G20ThreeCameraCalibrationNode._fit_axis_measurement(node, name, cycle) + ) + + count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( + node + ) + + # Trajectory-free synthetic rows cannot pass the hard gates: the task is + # dropped at import time instead of failing the final fit at the very + # end and dragging the session back to re-collect it. + assert count == 0 + assert node.sweep_index == 0 + assert node.resumed_task_keys == () + assert all( + not records for records in node.records_by_joint.values() + ) + restored = [ + json.loads(line) for line in current_raw.read_text().splitlines() + ] + import_record = next( + row + for row in restored + if row["kind"] == "resume_checkpoint_import" + ) + assert import_record["revalidation_dropped_tasks"] + assert ( + import_record["revalidation_dropped_tasks"][0]["task"] + == profile.sweep_specs[0].key + ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_pnp.py b/src/g20_thumb_apriltag_calibration/test/test_pnp.py index 1e6c7f8..050b3d2 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_pnp.py +++ b/src/g20_thumb_apriltag_calibration/test/test_pnp.py @@ -234,6 +234,70 @@ def _pose( ) +def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("base", "mcp", "ip"), + adjacent_pairs=(("base", "mcp"), ("mcp", "ip")), + maximum_pose_jump_rad=np.deg2rad(360.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(1000.0), + relative_translation_scale_m=1.0, + reprojection_scale_px=0.1, + reprojection_weight=1.0, + reset_after_seconds=5.0, + coupled_rotation_pairs=( + ("base", "mcp", "mcp", "ip", 1.03), + ), + coupled_rotation_scale_rad=np.deg2rad(3.0), + maximum_coupled_rotation_residual_rad=np.deg2rad(7.5), + ) + baseline = { + "base": (_pose(0.0, 0.00, 0.05),), + "mcp": (_pose(0.0, 0.03, 0.05),), + "ip": (_pose(0.0, 0.06, 0.05),), + } + selected, reason = tracker.select( + baseline, + stamp_ns=1_000_000_000, + trajectory_command_u8=255, + trajectory_direction="decreasing", + ) + assert selected is not None + assert reason == "" + + driver = _pose(10.0, 0.03, 0.05) + measured_ip = _pose(20.3, 0.06, 0.20) + lower_reprojection_mirror = _pose(30.0, 0.06, 0.05) + selected, reason = tracker.select( + { + "base": (_pose(0.0, 0.00, 0.05),), + "mcp": (driver,), + "ip": (lower_reprojection_mirror, measured_ip), + }, + stamp_ns=1_033_000_000, + trajectory_command_u8=128, + trajectory_direction="decreasing", + ) + + assert reason == "" + assert selected is not None + assert selected["ip"] == measured_ip + + fallback, fallback_reason = tracker.select( + { + "base": (_pose(0.0, 0.00, 0.05),), + "mcp": (driver,), + "ip": (lower_reprojection_mirror,), + }, + stamp_ns=1_066_000_000, + trajectory_command_u8=128, + trajectory_direction="decreasing", + ) + assert fallback is not None + assert fallback_reason == "" + assert fallback["ip"] == lower_reprojection_mirror + + def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None: tracker = SquareTagGroupPoseTracker( roles=("t0", "t3", "t4", "t5"), @@ -324,6 +388,58 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None: assert selected == {"t4": return_t4, "t5": return_t5} +def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("parent", "child"), + adjacent_pairs=(("parent", "child"),), + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + ) + parent = _pose(0.0, 0.00, 0.05) + for stamp, command, angle in ( + (1_000_000_000, 255, 20.0), + (1_033_000_000, 64, 45.0), + (1_066_000_000, 0, 50.0), + ): + selected, reason = tracker.select( + {"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)}, + stamp_ns=stamp, + trajectory_command_u8=command, + trajectory_direction="decreasing", + ) + assert reason == "" + assert selected is not None + + # The true return contains 3 deg of real hysteresis along the learned + # y-axis. The lower-error mirror candidate is temporally smoother but + # adds a 2 deg tilt outside that physical motion axis. + true_return = _pose(42.0, 0.03, 0.20) + smoother_mirror = SquareTagPose( + quaternion_xyzw=tuple( + Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat() + ), + translation_xyz_m=(0.03, 0.0, 0.25), + reprojection_error_px=0.01, + ) + selected, reason = tracker.select( + { + "parent": (parent,), + "child": (smoother_mirror, true_return), + }, + stamp_ns=1_099_000_000, + trajectory_command_u8=64, + trajectory_direction="increasing", + ) + + assert reason == "" + assert selected == {"parent": parent, "child": true_return} + + def test_group_tracker_initializes_from_multiple_static_frames() -> None: tracker = SquareTagGroupPoseTracker( roles=("parent", "child"), @@ -372,6 +488,63 @@ def test_group_tracker_initializes_from_multiple_static_frames() -> None: ] < np.deg2rad(1.0) +def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("parent", "child"), + adjacent_pairs=(("parent", "child"),), + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + ) + parent = _pose(0.0, 0.00, 0.05) + anchored_child = _pose(20.0, 0.03, 0.20) + for index in range(8): + selected, reason = tracker.select( + {"parent": (parent,), "child": (anchored_child,)}, + stamp_ns=1_000_000_000 + index * 33_000_000, + trajectory_command_u8=255, + trajectory_direction="decreasing", + ) + assert reason == "" + assert selected == {"parent": parent, "child": anchored_child} + + tracker.reset(preserve_task_reference=True) + lower_error_mirror = _pose(5.0, 0.03, 0.01) + for index in range(8): + selected, reason = tracker.select( + { + "parent": (parent,), + "child": (lower_error_mirror, anchored_child), + }, + stamp_ns=2_000_000_000 + index * 33_000_000, + trajectory_command_u8=255, + trajectory_direction="decreasing", + ) + + assert reason == "" + assert selected == {"parent": parent, "child": anchored_child} + assert tracker.last_initialization_quality["task_reference_used"] == "true" + + tracker.reset() + for index in range(8): + selected, reason = tracker.select( + { + "parent": (parent,), + "child": (lower_error_mirror, anchored_child), + }, + stamp_ns=3_000_000_000 + index * 33_000_000, + trajectory_command_u8=255, + trajectory_direction="decreasing", + ) + assert reason == "" + assert selected == {"parent": parent, "child": lower_error_mirror} + + def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None: tracker = SquareTagGroupPoseTracker( roles=("mcp", "pip", "dip"), 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 a956536..f0c8ad0 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,36 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None: assert "/g20_calibration/resume" in text +def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None: + explanation, action = three_camera_reason_zh( + "PAUSED", "combination_pose_prediction_failed", {} + ) + + assert "多关节组合姿态" in explanation + assert "未分类原因码" not in explanation + assert "combination_validation_failure" in action + assert "不要重新采集16个单关节任务" in action + + +def test_multiview_failure_names_the_camera_specific_joint() -> None: + explanation, action = three_camera_reason_zh( + "PAUSED", + "sweep_bin_gap_too_large:pinky_mcp_roll_side", + { + "sample": { + "maximum_bin_gap": 20, + "maximum_bin_gap_start_u8": 100, + "maximum_bin_gap_end_u8": 120, + "allowed_maximum_bin_gap": 16, + } + }, + ) + + assert "小指MCP侧摆（侧面校验）" in explanation + assert "100→120" in explanation + assert "resume" in action + + def test_preflight_lists_missing_tags_in_chinese() -> None: payload = { "state": "PREFLIGHT", @@ -137,6 +167,82 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None: assert "运动采样：" not in text +def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None: + payload = { + "state": "PAUSED", + "reason": "joint_fit_check_failed", + "progress": 0.15, + "completed_sweeps": 24, + "total_sweeps": 160, + "active": { + "kind": "fit_failure", + "view": "front", + "motor_index": 15, + "joints": ["thumb_ip"], + "attempt": 3, + "directions_to_rescan": 6, + "failures": [ + { + "joint": "thumb_ip", + "metric": "baseline_hysteresis_deg", + "actual": 1.34, + "limit": 0.5, + "comparison": "maximum", + "cycle_values_deg": [1.34, 0.04, 0.14], + } + ], + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "baseline正反程关节角差为1.34°" in text + assert "要求不超过0.50°" in text + assert "各轮=1.34°/0.04°/0.14°" in text + assert "未知原因" not in text + + +def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None: + payload = { + "state": "PAUSED", + "reason": "sweep_bin_gap_too_large", + "progress": 0.1, + "completed_sweeps": 1, + "total_sweeps": 10, + "active": { + "kind": "sweep", + "view": "top", + "motor_index": 10, + "joints": ["thumb_cmc_yaw"], + "cycle": 1, + "repetitions": 3, + "start_u8": 255, + "target_u8": 0, + "actual_u8": 3.0, + "valid_frames": 610, + "detection_frames": 644, + "detection_valid_frames": 610, + "detection_rate": 610 / 644, + "sample": { + "minimum_u8": 3.0, + "maximum_u8": 252.0, + "maximum_bin_gap": 17, + "maximum_bin_gap_start_u8": 99, + "maximum_bin_gap_end_u8": 116, + "allowed_maximum_bin_gap": 16, + }, + }, + "views": {}, + } + + text = render_three_camera_status_text_zh(payload) + + assert "最大空缺为17，（99→116）" in text + assert "本方向Tag检出：94.7%（610/644帧）" in text + + def test_zero_model_failure_explains_that_rescan_will_not_help() -> None: payload = { "state": "PAUSED", @@ -210,6 +316,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None: "reason": ( "motor_state_stalled:return_baseline:motor_index=10:" "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:" + "timeout_seconds=2.000:" "error_u8=5.000" ), "progress": 0.0, @@ -224,6 +331,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None: "actual_u8": 250.0, "error_u8": 5.0, "tolerance_u8": 4.0, + "timeout_seconds": 2.0, }, "views": {}, "result_path": "", @@ -231,7 +339,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None: text = render_three_camera_status_text_zh(payload) - assert "电机10反馈连续8秒" in text + assert "电机10反馈连续2秒" in text assert "目标255.0、实际250.0、误差5.000 u8" in text assert "允许容差±4.0 u8" in text assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text @@ -371,3 +479,15 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None: assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text assert "SDK报告[15, 5, 15, 15, 15]" in text assert "自动重试：当前方向已自动重扫1/2次" in text + + +def test_same_finger_transition_explains_that_clearance_stays_parked() -> None: + explanation, action = three_camera_reason_zh( + "RETURN_BASELINE", + "holding_same_finger_clearance_before_next_task", + {}, + ) + + assert "继续保持当前避让姿态" in explanation + assert "只调整被测关节" in explanation + assert "不要手动展开" in action 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 72cf3d9..5abe5dc 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 @@ -1,18 +1,27 @@ import json import math +from collections import deque from dataclasses import replace from types import SimpleNamespace import numpy as np +import pytest from scipy.spatial.transform import Rotation 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 ( + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_HAND_PROFILE, RIGHT_HAND_PROFILE, SWEEP_SPECS, + THREE_CAMERA_BASELINE_COMMAND, + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, ) from g20_thumb_apriltag_calibration.three_camera_node import ( FrameObservation, @@ -20,7 +29,17 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( STATE_PREPARE_SWEEP, STATE_SWEEP, SweepItem, + _classify_cross_view_roll_hysteresis, + _combination_observable_joints, + _fit_failure_is_systematic, + _frames_cover_sweep_joints, _overall_progress, + _selected_pose_qualities, + _sweep_views, +) +from g20_thumb_apriltag_calibration.pnp import SquareTagPose +from g20_thumb_apriltag_calibration.urdf_zero import ( + get_zero_calibration_profile, ) @@ -49,6 +68,335 @@ def _frame(view: str, state_u8: list[float]) -> FrameObservation: ) +def _joint_frame( + view: str, state_u8: list[float], joint_name: str +) -> FrameObservation: + identity_pose = { + "translation_xyz_m": [0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + } + return replace( + _frame(view, state_u8), + joint_vectors_xyz_m={joint_name: (0.02, 0.0, 0.0)}, + image_vectors_xy_px={joint_name: (20.0, 0.0)}, + joint_quaternions_xyzw={joint_name: (0.0, 0.0, 0.0, 1.0)}, + parent_poses_common={joint_name: identity_pose}, + child_poses_common={joint_name: identity_pose}, + joint_reprojection_error_px={joint_name: 0.05}, + ) + + +def test_right_roll_uses_one_motion_with_two_camera_observations() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + state = [255.0] * 20 + front = _joint_frame("front", state, "pinky_mcp_roll") + side = _joint_frame("side", state, "pinky_mcp_roll_side") + + assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("front", "side") + assert not _frames_cover_sweep_joints([front] * 3, spec, 3) + assert _frames_cover_sweep_joints([front] * 3 + [side] * 3, spec, 3) + + +def test_right_roll_requires_both_views_task_local_tags() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={ + "front": SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), + preflight_roles=("front_base",), + ), + "side": SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["side"]), + preflight_roles=("side_base",), + ), + }, + 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", "pinky_roll") + assert G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) == ("side_base", "pinky_pip") + + +def test_multiview_roll_uses_locked_front_base_but_live_moving_tag() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "middle_roll_multiview" + ) + locked_pose = SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 + ) + front = SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), + preflight_roles=("front_base",), + locked_base_pose=locked_pose, + locked_base_center_xy_px=(100.0, 200.0), + locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={"front": front}, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + active_combination_validation=None, + ) + node._locked_base_role_for_active_capture = lambda view: ( + G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( + node, view + ) + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "front" + ) + live = G20ThreeCameraCalibrationNode._live_required_roles_for_view( + node, "front", required + ) + + assert required == ("front_base", "middle_roll") + assert live == ("middle_roll",) + + +def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "middle_pitch_side" + ) + front = SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), + preflight_roles=("front_base",), + locked_base_pose=SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 + ), + locked_base_center_xy_px=(100.0, 200.0), + locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={"front": front}, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + active_combination_validation=None, + ) + + assert ( + G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( + node, "front" + ) + == "front_base" + ) + + +def test_hidden_locked_base_uses_baseline_quality_without_becoming_live() -> None: + locked_pose = SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.17 + ) + moving_pose = SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.02, 0.0, 1.0), 0.23 + ) + live = {"middle_roll": TagQuality(0, 90.0, 32.0)} + + combined = _selected_pose_qualities( + {"front_base": locked_pose, "middle_roll": moving_pose}, + live, + locked_base_role="front_base", + locked_base_quality=TagQuality(0, 100.0, 35.0, 0.11), + ) + + assert set(live) == {"middle_roll"} + assert combined["front_base"].reprojection_error_px == 0.17 + assert combined["middle_roll"].reprojection_error_px == 0.23 + + +def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None: + observations = deque(maxlen=60) + for index in range(30): + observations.append( + ( + SquareTagPose( + (0.0, 0.0, 0.0, 1.0), + (0.001 * (index % 2), 0.0, 1.0), + 0.1, + ), + (100.0 + index % 2, 200.0), + TagQuality(0, 100.0, 30.0, 0.1), + ) + ) + views = { + view: SimpleNamespace( + fixed_base_observations=deque(observations, maxlen=60), + locked_base_pose=None, + locked_base_center_xy_px=None, + locked_base_quality=None, + view_tags={ + {"front": "front_base", "side": "side_base", "top": "top_base"}[ + view + ]: {"front": 0, "side": 4, "top": 8}[view] + }, + ) + for view in ("front", "side", "top") + } + node = SimpleNamespace( + preflight_frames=60, + views=views, + raw_path=tmp_path / "raw_samples.jsonl", + ) + + assert G20ThreeCameraCalibrationNode._lock_fixed_base_references(node) + assert views["front"].locked_base_pose is not None + assert views["front"].locked_base_center_xy_px == (100.5, 200.0) + events = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert {event["tag_id"] for event in events} == {0, 4, 8} + + +def test_right_roll_checkpoint_persists_each_camera_independently( + tmp_path, +) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + state = [255.0] * 20 + state[spec.motor_index] = 224.0 + frames = ( + [_joint_frame("front", state, "pinky_mcp_roll") for _ in range(3)] + + [ + _joint_frame("side", state, "pinky_mcp_roll_side") + for _ in range(3) + ] + ) + records = {name: [] for name in spec.joints} + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + sweep_attempts={spec.key: 1}, + command_records_by_joint=records, + raw_path=tmp_path / "raw_samples.jsonl", + ) + + G20ThreeCameraCalibrationNode._record_command_checkpoint( + node, item, 224, frames + ) + + assert [records[name][0]["view"] for name in spec.joints] == [ + "front", + "side", + ] + assert all(records[name][0]["valid_frames"] == 3 for name in spec.joints) + + +def test_final_steady_checkpoint_unlatches_checkpoint_mode() -> None: + node = SimpleNamespace( + sweep_checkpoint_commands=deque(), + sweep_checkpoint_target_u8=0, + sweep_checkpoint_mode=True, + ) + + advanced = G20ThreeCameraCalibrationNode._publish_next_checkpoint( + node, 1.0 + ) + + assert advanced is False + assert node.sweep_checkpoint_target_u8 is None + assert node.sweep_checkpoint_mode is False + + +def test_steady_checkpoint_accepts_stable_command_feedback_deadband() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "thumb_cmc_yaw_top" + ) + node = SimpleNamespace( + latest_state_u8=(), + baseline_command=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + endpoint_tolerance_u8=2.0, + thumb_yaw_zero_endpoint_tolerance_u8=4.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, + steady_checkpoint_command_feedback_tolerance_u8=8.0, + steady_checkpoint_maximum_feedback_range_u8=2.0, + ) + node._endpoint_tolerance_for_spec = lambda selected, command: ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, selected, command + ) + ) + state = list( + build_calibration_motion_command( + spec, + 32, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + ) + ) + state[10] = 35.0 + + assert not G20ThreeCameraCalibrationNode._motion_command_reached( + node, spec, 32, tuple(state) + ) + assert G20ThreeCameraCalibrationNode._steady_checkpoint_reached( + node, spec, 32, tuple(state) + ) + + state[10] = 41.0 + assert not G20ThreeCameraCalibrationNode._steady_checkpoint_reached( + node, spec, 32, tuple(state) + ) + + +def test_steady_checkpoint_requires_feedback_to_stop_moving() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "thumb_cmc_yaw_top" + ) + node = SimpleNamespace( + steady_checkpoint_maximum_feedback_range_u8=2.0 + ) + stable_frames = [] + moving_frames = [] + for value in (35.0, 35.5, 35.0): + state = [255.0] * 20 + state[10] = value + stable_frames.append(_frame("top", state)) + for value in (38.0, 36.0, 35.0): + state = [255.0] * 20 + state[10] = value + moving_frames.append(_frame("top", state)) + + assert G20ThreeCameraCalibrationNode._steady_checkpoint_feedback_is_stable( + node, spec, stable_frames + ) + assert not G20ThreeCameraCalibrationNode._steady_checkpoint_feedback_is_stable( + node, spec, moving_frames + ) + + def test_right_thumb_pitch_requires_only_front_base_and_thumb_tag() -> None: profile = RIGHT_HAND_PROFILE spec = next(item for item in profile.sweep_specs if item.motor_index == 0) @@ -92,6 +440,82 @@ def test_right_side_preflight_requires_only_pinky_chain_tags() -> None: assert required == ("side_base", "pinky_mcp", "pinky_pip", "pinky_dip") +def test_right_19_preflight_requires_only_fixed_palm_tag() -> None: + profile = RIGHT_19_HAND_PROFILE + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["side"]), + preflight_roles=profile.preflight_view_roles["side"], + ) + node = SimpleNamespace( + profile=profile, + views={"side": runtime}, + active_sweep=None, + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) + + assert required == ("side_base",) + + +def test_right_19_combination_allows_occluded_neighbouring_fingers() -> None: + profile = RIGHT_19_HAND_PROFILE + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["side"]), + preflight_roles=profile.preflight_view_roles["side"], + ) + node = SimpleNamespace( + profile=profile, + views={"side": runtime}, + active_combination_validation=SimpleNamespace(name="index_middle"), + active_sweep=None, + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) + observable = _combination_observable_joints( + profile, + "side", + ("side_base", "index_pip", "index_dip"), + ) + + assert required == ("side_base",) + assert observable == ("index_pip",) + + +def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None: + profile = RIGHT_19_HAND_PROFILE + spec = next( + item + for item in profile.sweep_specs + if item.task_name == "index_pip_side" + ) + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["side"]), + preflight_roles=profile.preflight_view_roles["side"], + ) + node = SimpleNamespace( + profile=profile, + views={"side": runtime}, + active_combination_validation=None, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) + + assert required == ("side_base", "index_pip") + + def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None: profile = RIGHT_HAND_PROFILE spec = next( @@ -188,6 +612,252 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None: assert published[0][7:10] == [0, 0, 0] +def test_right_19_roll_sweep_stops_at_settled_baseline_first() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) + endpoint_state = list(baseline) + endpoint_state[9] = 255.0 + published: list[list[int]] = [] + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + active_sweep=item, + baseline_command=baseline, + sweep_frames=[], + sweep_start_frames=[_frame("front", endpoint_state)], + sweep_baseline_frames=[], + _publish_command=lambda command: published.append(command), + _motion_command_error_u8=lambda selected, command: 128.0, + _reset_motion_progress=lambda now, error: None, + ) + + G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0) + + assert node.sweep_baseline_pending is True + assert node.sweep_baseline_hold_since is None + assert published[0][9] == 127 + + +def test_dedicated_baseline_buffer_ignores_moving_arrival_frame() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + baseline_state = build_calibration_motion_command( + spec, + 127, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + ) + moving_state = list(baseline_state) + moving_state[9] = 130.0 + node = SimpleNamespace( + state=STATE_SWEEP, + profile=RIGHT_19_HAND_PROFILE, + active_sweep=item, + baseline_command=THREE_CAMERA_BASELINE_COMMAND, + endpoint_tolerance_u8=2.0, + latest_state_u8=tuple(baseline_state), + sweep_frames=[], + sweep_baseline_frames=[], + sweep_baseline_pending=True, + sweep_baseline_hold_since=None, + baseline_hold_seconds=0.5, + sweep_last_valid_at=0.0, + ) + node._motion_command_reached = lambda selected, command, state=None: ( + G20ThreeCameraCalibrationNode._motion_command_reached( + node, selected, command, state + ) + ) + node._endpoint_tolerance_for_spec = lambda selected, endpoint: ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, selected, endpoint + ) + ) + + G20ThreeCameraCalibrationNode._accept_frame( + node, _frame("front", baseline_state) + ) + assert node.sweep_baseline_frames == [] + + node.sweep_baseline_hold_since = 1.0 + G20ThreeCameraCalibrationNode._accept_frame( + node, _frame("front", moving_state) + ) + G20ThreeCameraCalibrationNode._accept_frame( + node, _frame("front", baseline_state) + ) + + assert len(node.sweep_baseline_frames) == 1 + assert node.sweep_baseline_frames[0].state_u8[9] == 127 + + +def test_dedicated_baseline_hold_is_persisted_separately(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + item = SweepItem(spec, 2, DIRECTION_INCREASING) + state = build_calibration_motion_command( + spec, + 127, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + ) + frames = [ + replace( + _frame("front", state), + joint_quaternions_xyzw={ + "pinky_mcp_roll": (0.0, 0.0, 0.0, 1.0) + }, + joint_vectors_xyz_m={"pinky_mcp_roll": (0.02, 0.0, 0.0)}, + image_vectors_xy_px={"pinky_mcp_roll": (20.0, 0.0)}, + parent_poses_common={ + "pinky_mcp_roll": { + "translation_xyz_m": [0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + } + }, + child_poses_common={ + "pinky_mcp_roll": { + "translation_xyz_m": [0.02, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + } + }, + joint_reprojection_error_px={"pinky_mcp_roll": 0.05}, + ) + for _ in range(3) + ] + frames.extend( + replace( + frame, + view="side", + joint_quaternions_xyzw={ + "pinky_mcp_roll_side": (0.0, 0.0, 0.0, 1.0) + }, + joint_vectors_xyz_m={"pinky_mcp_roll_side": (0.03, 0.0, 0.0)}, + image_vectors_xy_px={"pinky_mcp_roll_side": (30.0, 0.0)}, + parent_poses_common={ + "pinky_mcp_roll_side": frame.parent_poses_common[ + "pinky_mcp_roll" + ] + }, + child_poses_common={ + "pinky_mcp_roll_side": frame.child_poses_common[ + "pinky_mcp_roll" + ] + }, + joint_reprojection_error_px={"pinky_mcp_roll_side": 0.05}, + ) + for frame in list(frames) + ) + records = {"pinky_mcp_roll": [], "pinky_mcp_roll_side": []} + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=THREE_CAMERA_BASELINE_COMMAND, + sweep_baseline_frames=frames, + minimum_baseline_hold_frames=3, + baseline_hold_seconds=0.5, + sweep_attempts={spec.key: 2}, + baseline_records_by_joint=records, + raw_path=tmp_path / "raw_samples.jsonl", + ) + + G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold(node, item) + + assert len(records["pinky_mcp_roll"]) == 1 + record = records["pinky_mcp_roll"][0] + assert record["kind"] == "baseline_hold_sample" + assert record["cycle"] == 2 + assert record["direction"] == DIRECTION_INCREASING + assert record["command_u8"] == 127 + assert record["valid_frames"] == 3 + assert record["relative_translation_xyz_m"] == [0.02, 0.0, 0.0] + assert record["parent_pose_common"]["translation_xyz_m"] == [0.0] * 3 + + +def test_cross_view_roll_hysteresis_classification() -> None: + assert _classify_cross_view_roll_hysteresis( + [0.99, 1.00, 0.98, 0.99], + [0.91, 0.94, 0.93, 0.92], + limit_deg=0.5, + ) == "both_views_confirm_direction_dependent_pose" + assert _classify_cross_view_roll_hysteresis( + [0.99] * 4, + [0.20] * 4, + limit_deg=0.5, + ) == "front_only_difference_check_roll_tag_bracket_or_front_pnp" + + +def test_diagnostic_mode_uses_one_multiview_physical_task() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + node = SimpleNamespace( + cross_view_roll_diagnostic_finger="pinky", + ) + + role = G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + node, spec + ) + + assert role == "multiview" + assert spec.joints == ("pinky_mcp_roll", "pinky_mcp_roll_side") + + +def test_diagnostic_completion_locks_publication(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + pauses: list[str] = [] + node = SimpleNamespace( + cross_view_roll_diagnostic_finger="pinky", + baseline_maximum_hysteresis_rad=math.radians(0.5), + raw_path=tmp_path / "raw_samples.jsonl", + fit_failure={}, + cross_view_roll_diagnostic_result={}, + _roll_baseline_hysteresis_degrees=lambda name: ( + [0.99, 1.00, 0.98, 0.99] + if name == "pinky_mcp_roll" + else [0.92, 0.94, 0.93, 0.91] + ), + _pause=lambda reason: pauses.append(reason), + ) + + G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( + node, + spec, + [ + { + "joint": "pinky_mcp_roll_side", + "metric": "rotation_circle_axis_difference_deg", + "actual": 24.3, + "limit": 1.0, + } + ], + ) + + result = node.cross_view_roll_diagnostic_result + assert result["publication_locked"] is True + assert result["interpretation"] == ( + "both_views_confirm_direction_dependent_pose" + ) + assert result["side_quality_failures"][0]["actual"] == 24.3 + assert pauses == ["cross_view_roll_diagnostic_complete"] + + def test_joint_specific_zero_endpoint_deadbands() -> None: yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10) index_roll = next(item for item in SWEEP_SPECS if item.motor_index == 6) @@ -223,6 +893,44 @@ def test_joint_specific_zero_endpoint_deadbands() -> None: ) == 2.0 +def test_pose_transition_uses_slow_roll_speed_for_motor_six() -> None: + state = [255.0] * 20 + target = [255] * 20 + state[6] = 172.0 + target[6] = 127 + node = SimpleNamespace( + latest_state_u8=tuple(state), + index_roll_calibration_speed=5, + index_flex_calibration_speed=10, + _normal_speed_profile=lambda: [15] * 5, + ) + + speeds = G20ThreeCameraCalibrationNode._transition_speed_profile( + node, target + ) + + assert speeds == [15, 5, 15, 15, 15] + + +def test_pose_transition_uses_flex_speed_for_pitch_and_pip() -> None: + state = [255.0] * 20 + target = [255] * 20 + target[3] = 0 + target[18] = 0 + node = SimpleNamespace( + latest_state_u8=tuple(state), + index_roll_calibration_speed=5, + index_flex_calibration_speed=10, + _normal_speed_profile=lambda: [15] * 5, + ) + + speeds = G20ThreeCameraCalibrationNode._transition_speed_profile( + node, target + ) + + assert speeds == [15, 15, 15, 10, 15] + + def test_pinky_pip_command_zero_accepts_firmware_feedback_five() -> None: pinky_pip = next( item for item in RIGHT_HAND_PROFILE.sweep_specs @@ -296,6 +1004,67 @@ def test_right_baseline_and_auxiliary_accept_thumb_yaw_feedback_250() -> None: ) +def test_safe_waypoint_ignores_reserved_channels_and_reports_real_motor() -> None: + state = [ + 255.0, 254.0, 254.0, 254.0, 254.0, 254.0, + 127.0, 127.0, 127.0, 127.0, 253.0, + 0.0, 0.0, 0.0, 0.0, + 255.0, 253.0, 253.0, 253.0, 253.0, + ] + command = [int(round(value)) for value in state] + command[5] = 255 + # Reserved SDK feedback is zero even if an old full command contains 255. + command[11:15] = [255, 255, 255, 255] + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + latest_state_u8=tuple(state), + endpoint_tolerance_u8=2.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, + ) + + assert G20ThreeCameraCalibrationNode._command_vector_reached(node, command) + assert G20ThreeCameraCalibrationNode._command_vector_error_u8( + node, command + ) == 1.0 + + command[3] = 240 + details = G20ThreeCameraCalibrationNode._command_vector_error_details( + node, command, "prepare_motor_0" + ) + assert details["motor_index"] == 3 + assert details["actual_u8"] == 254.0 + assert details["target_u8"] == 240.0 + assert details["error_u8"] == 14.0 + + +def test_feedback_inside_endpoint_tolerance_does_not_create_noop_transition() -> None: + state = [ + 254.0, 254.0, 254.0, 254.0, 254.0, 254.0, + 127.0, 127.0, 127.0, 127.0, 250.0, + 0.0, 0.0, 0.0, 0.0, + 255.0, 254.0, 254.0, 254.0, 254.0, + ] + target = [255] * 20 + target[6:10] = [127] * 4 + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + endpoint_tolerance_u8=2.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, + ) + + snapped = G20ThreeCameraCalibrationNode._snap_reached_state_to_command( + node, state, target + ) + + controlled = { + spec.motor_index for spec in RIGHT_19_HAND_PROFILE.joint_specs.values() + } + assert all(snapped[index] == target[index] for index in controlled) + assert snapped[11:15] == (0, 0, 0, 0) + + def test_thumb_yaw_command_zero_accepts_feedback_four_only_for_swept_motor() -> None: yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10) state = [255.0] * 20 @@ -407,6 +1176,348 @@ def test_thumb_yaw_clearance_is_released_only_for_normal_transition() -> None: assert command[5] == 255 +def test_next_cycle_keeps_side_clearance_pose_parked() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "ring_roll_multiview" + ) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + next_item = SweepItem(spec, 1, DIRECTION_DECREASING) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=tuple(baseline), + sweep_items=[next_item], + sweep_index=0, + retry_sweep_items=[], + ) + + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "next_cycle" + ) + + # Ring and pinky roll remain at their safe 127 baselines. The already- + # cleared pinky remains bent instead of being unfolded and bent again. + assert command[8] == 127 + assert command[4] == 0 + assert command[9] == 127 + assert command[19] == 0 + assert command[1] == 255 + assert command[6] == 255 + + +def test_middle_fit_retry_keeps_side_clearance_pose_parked() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "middle_roll_multiview" + ) + baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) + queued = SweepItem(spec, 0, DIRECTION_DECREASING) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=baseline, + active_sweep=None, + retry_sweep_items=[queued], + ) + + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "resume_sweep" + ) + + expected = tuple( + build_calibration_motion_command( + spec, + queued.start_u8, + baseline=baseline, + profile=RIGHT_19_HAND_PROFILE, + ) + ) + assert command == expected + # Pinky and ring remain flexed out of both cameras instead of unfolding + # to the global baseline and immediately being flexed again. + assert command[3] == 0 and command[18] == 0 + assert command[4] == 0 and command[19] == 0 + assert command[8] == 127 and command[9] == 127 + assert command[7] == queued.start_u8 + + +def test_middle_fit_retry_has_no_unfold_refold_waypoint() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "middle_roll_multiview" + ) + baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) + queued = SweepItem(spec, 0, DIRECTION_DECREASING) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=baseline, + active_sweep=None, + retry_sweep_items=[queued], + ) + current = list( + build_calibration_motion_command( + spec, + 255, + baseline=baseline, + profile=RIGHT_19_HAND_PROFILE, + ) + ) + + target = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "resume_sweep" + ) + returns = build_calibration_return_waypoints( + target, + current_command=current, + profile=RIGHT_19_HAND_PROFILE, + ) + after_return = returns[-1] + preparations = build_calibration_preparation_waypoints( + spec, + queued.start_u8, + current_command=after_return, + baseline=baseline, + profile=RIGHT_19_HAND_PROFILE, + ) + + for waypoint in (*returns, *preparations): + assert waypoint[3] == 0 and waypoint[18] == 0 + assert waypoint[4] == 0 and waypoint[19] == 0 + + +def test_same_finger_next_task_keeps_neighbour_clearance_parked() -> None: + ring_roll_multiview = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "ring_roll_multiview" + ) + ring_pitch_side = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "ring_pitch_side" + ) + baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) + next_item = SweepItem( + ring_pitch_side, -1, DIRECTION_DECREASING, precheck=True + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=baseline, + sweep_items=[next_item], + sweep_index=0, + retry_sweep_items=[], + ) + + transition = ( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + node, ring_roll_multiview + ) + ) + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, transition + ) + + assert transition == "next_task_same_finger" + # Pinky was bent once for ring side visibility and stays parked through + # ring roll, MCP pitch and PIP instead of unfolding between tasks. + assert command[9] == 127 + assert command[4] == 0 + assert command[19] == 0 + assert command[8] == 127 + assert command[3] == 255 + assert command[18] == 255 + + +def test_changing_finger_still_requires_global_safe_transition() -> None: + ring_pip = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "ring_pip_side" + ) + middle_roll = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "middle_roll_multiview" + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + sweep_items=[SweepItem(middle_roll, -1, DIRECTION_DECREASING, True)], + sweep_index=0, + retry_sweep_items=[], + ) + + assert ( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + node, ring_pip + ) + == "next_sweep" + ) + + +def test_retry_return_anchors_roll_order_on_failed_target_finger() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "pinky_roll_multiview" + ) + node = SimpleNamespace( + retry_sweep_items=[ + SweepItem(spec, 0, DIRECTION_DECREASING) + ], + sweep_items=[], + sweep_index=0, + active_sweep=None, + ) + + anchor = G20ThreeCameraCalibrationNode._return_anchor_roll_motor(node) + + assert anchor == 9 + + +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" + ) + 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=205, + maximum_bin_gap=3, + valid_frames=320, + ) + + assert math.isclose(node.formal_speed_scales[spec.key], 22.0 / 15.0) + node.active_sweep = SweepItem(spec, 0, DIRECTION_DECREASING) + speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) + assert speeds[0] == 22 + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["kind"] == "task_formal_speed_selected" + assert event["base_speed"] == 15 + assert event["formal_speed"] == 22 + + +def test_precheck_large_gap_keeps_conservative_formal_speed(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "thumb_cmc_yaw_top" + ) + 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=17, + valid_frames=640, + ) + + 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 + + +def test_roll_precheck_never_accelerates_strict_backlash_scan(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.task_name == "pinky_roll_multiview" + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + active_sweep=None, + active_sweep_is_fit_retry=False, + 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=100, + maximum_bin_gap=4, + valid_frames=140, + ) + + 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" + + +def test_fit_retry_uses_base_speed_instead_of_adaptive_speed() -> 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=SweepItem(spec, 1, DIRECTION_DECREASING), + active_sweep_is_fit_retry=True, + normal_calibration_speed=15, + index_roll_calibration_speed=5, + index_flex_calibration_speed=10, + formal_speed_scales={spec.key: 1.5}, + sweep_retry_counts={}, + ) + + speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) + + assert speeds[0] == 15 + + def test_thumb_yaw_recovery_zero_accepts_feedback_four() -> None: yaw = next( item for item in RIGHT_HAND_PROFILE.sweep_specs @@ -502,6 +1613,7 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: passive_maximum_cycle_travel_difference_rad=math.radians(10.0), maximum_monotonic_correction_rad=math.radians(2.0), maximum_hysteresis_rad=math.radians(5.0), + baseline_maximum_hysteresis_rad=math.radians(0.5), passive_maximum_monotonic_correction_rad=math.radians(3.0), passive_maximum_hysteresis_rad=math.radians(7.5), baseline_command=[255] * 20, @@ -525,6 +1637,63 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: return node +def test_right_19_end_on_flexion_uses_image_curve_not_planar_pnp_tilt() -> None: + records = _image_cycle_records([math.radians(100.0)] * 3) + # Reproduce a command-dependent out-of-axis PnP tilt while preserving the + # accurately observed projected Tag-centre circle. + for record in records: + command = int(record["command_u8"]) + angle = math.radians(100.0) * (255.0 - command) / 255.0 + rotation = Rotation.from_rotvec([0.0, 0.0, angle]) * Rotation.from_rotvec( + [math.radians(12.0) * math.sin(angle), 0.0, 0.0] + ) + record["relative_quaternion_xyzw"] = rotation.as_quat().tolist() + + node = _fit_check_node({"pinky_pip": records, "pinky_dip": records}) + node.profile = RIGHT_19_HAND_PROFILE + image_fit = G20ThreeCameraCalibrationNode._fit_joint_records( + node, "pinky_pip", records + ) + passive_fit = G20ThreeCameraCalibrationNode._fit_joint_records( + node, "pinky_dip", records + ) + + assert "pinky_pip" in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + 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 + # relative-orientation curve and must never inherit the PIP image motion. + assert passive_fit.circle["space"] == "relative_rotation_3d" + + +def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None: + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + pnp_maximum_pose_jump_rad=math.radians(35.0), + pnp_maximum_translation_jump_m=0.04, + pnp_group_initialization_frames=8, + pnp_group_normal_alignment_scale_rad=math.radians(5.0), + pnp_group_maximum_normal_alignment_rad=math.radians(15.0), + pnp_tracker_reset_seconds=5.0, + thumb_ip_pnp_coupling_multiplier=1.03, + thumb_ip_pnp_coupling_scale_rad=math.radians(3.0), + thumb_ip_pnp_maximum_coupling_residual_rad=math.radians(7.5), + ) + + tracker = G20ThreeCameraCalibrationNode._make_group_pose_tracker( + node, + "front", + ("front_base", "thumb_cmc", "thumb_mcp", "thumb_ip"), + ) + + assert tracker.coupled_rotation_pairs == ( + ("thumb_cmc", "thumb_mcp", "thumb_mcp", "thumb_ip", 1.03), + ) + assert tracker.maximum_coupled_rotation_residual_rad == pytest.approx( + math.radians(7.5) + ) + + def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) node = SimpleNamespace( @@ -730,6 +1899,86 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> assert [event["retry"] for event in events] == [1, 2, 3] +def test_visibility_precheck_does_not_require_dense_feedback_bins(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) + # Preserve the field observation: complete endpoints and midpoint, but a + # 17-u8 feedback gap between 99 and 116. + commands = [*range(0, 100), *range(116, 256)] + frames = [] + for command in commands: + state = [255.0] * 20 + state[10] = float(command) + frames.append(_frame("top", state)) + transitions: list[str] = [] + retries: list[str] = [] + node = SimpleNamespace( + active_sweep=item, + sweep_frames=frames, + minimum_sweep_bins=32, + maximum_bin_gap=16, + minimum_detection_rate=0.95, + views={"top": SimpleNamespace(valid_rate=1.0)}, + sweep_detection_total_frames=644, + sweep_detection_valid_frames=644, + 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: 5.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["kind"] == "task_visibility_precheck" + assert event["passed"] is True + assert event["detection_rate"] == 1.0 + + +def test_formal_sweep_still_rejects_17_u8_feedback_gap(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + commands = [*range(0, 100), *range(116, 256)] + frames = [] + for command in commands: + state = [255.0] * 20 + state[10] = float(command) + frames.append(_frame("top", state)) + retries: list[str] = [] + node = SimpleNamespace( + active_sweep=item, + sweep_frames=frames, + minimum_sweep_bins=32, + maximum_bin_gap=16, + _endpoint_tolerance_for_spec=lambda selected, endpoint: 5.0, + _retry_active_sweep_or_pause=lambda reason: retries.append(reason), + ) + + G20ThreeCameraCalibrationNode._finish_active_sweep(node) + + assert retries == ["sweep_bin_gap_too_large"] + + def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) transitions: list[str] = [] @@ -767,12 +2016,71 @@ def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: assert node.paused_reason == "joint_fit_check_failed" -def test_near_threshold_provisional_failure_warns_without_retry(tmp_path) -> None: +def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry( + tmp_path, +) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) + failures = [ + { + "joint": "thumb_ip", + "metric": "rotation_circle_axis_difference_deg", + "cycle": cycle, + "actual": actual, + "limit": 1.0, + "comparison": "maximum", + } + for cycle, actual in enumerate((3.9, 4.1, 4.0), start=1) + ] + assert _fit_failure_is_systematic(failures, 3) + transitions: list[str] = [] + pauses: list[str] = [] + node = SimpleNamespace( + sweep_items=_sweep_items(), + sweep_attempts={spec.motor_index: 1}, + retry_sweep_spec=None, + retry_resume_index=None, + fit_failure={}, + sweep_index=18, + raw_path=tmp_path / "raw_samples.jsonl", + repetitions=3, + automatic_fit_retry_limit=2, + paused_reason="", + reason="", + _begin_return_baseline=lambda after: transitions.append(after), + _pause=lambda reason: pauses.append(reason), + ) + + G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, spec, failures + ) + + assert transitions == [] + assert pauses == ["joint_fit_systematic_failure"] + assert node.fit_failure["recoverable_by_rescan"] is False + assert node.fit_failure["directions_to_rescan"] == 0 + + +def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None: + # In-band values are rejected by the final fit on the same records, so + # the first in-band result reschedules the task here instead of warning + # and deferring the rejection to the end of the session. spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) node = SimpleNamespace( raw_path=tmp_path / "raw_samples.jsonl", provisional_warning_ratio=1.25, reason="", + sweep_attempts={}, + retry_resume_index=None, + retry_sweep_spec=None, + retry_cycles=set(), + repetitions=3, + fit_failure={}, + sweep_index=0, + automatic_fit_retry_limit=2, + paused_reason="", + _prepare_failed_sweep_retry=lambda: None, + _begin_return_baseline=lambda after: None, + _pause=lambda reason: None, ) handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( node, @@ -789,10 +2097,14 @@ def test_near_threshold_provisional_failure_warns_without_retry(tmp_path) -> Non allow_warning=True, ) - assert handled is False - assert node.reason == "provisional_fit_warning_continuing" - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "provisional_fit_warning" + assert handled is True + events = [ + json.loads(line) + for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines() + ] + kinds = [event["kind"] for event in events] + assert "provisional_fit_warning_rescan" in kinds + assert "fit_failure" in kinds def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None: @@ -846,7 +2158,10 @@ def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: node, now=18.0, error_u8=18.0, context="sweep_motor_0" ) - assert pauses == ["motor_state_stalled:sweep_motor_0:error_u8=18.000"] + assert pauses == [ + "motor_state_stalled:sweep_motor_0:timeout_seconds=8.000:" + "error_u8=18.000" + ] event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) assert event["kind"] == "mechanical_motion_stall" assert event["context"] == "sweep_motor_0" @@ -924,6 +2239,41 @@ def test_provisional_fit_accepts_constrained_circle_for_image_joint() -> None: ] +def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + name = "pinky_mcp_roll_side" + spec = replace(spec, joints=(name,)) + records = _image_cycle_records( + [math.radians(47.0)] * 3, + depth_slope=2.0, + ) + node = _fit_check_node({name: records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {name: records} + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert not [ + failure + for failure in failures + if failure["metric"] + in { + "rotation_circle_axis_difference_deg", + "axis_plane_rms_mm", + } + ] + + def test_thumb_ip_axis_uses_same_cycle_thumb_mcp_direction() -> None: records = _image_cycle_records([math.radians(47.0)] * 3) node = _fit_check_node( @@ -975,6 +2325,43 @@ def test_provisional_fit_uses_orientation_constrained_circle_for_yaw() -> None: ] +def test_right_19_side_flexion_uses_end_on_orientation_axis() -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_pitch_side" + ) + name = "pinky_mcp_pitch" + records = _image_cycle_records( + [math.radians(47.0)] * 3, + depth_slope=0.30, + ) + node = _fit_check_node({name: records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + + measurement = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + node, name, 0 + ) + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert measurement.axis_direction_source == "rotation" + assert not [ + failure + for failure in failures + if failure["metric"] + in { + "rotation_circle_axis_difference_deg", + "axis_plane_rms_mm", + } + ] + + def test_provisional_fit_uses_active_and_passive_axis_model_limits() -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) records = _image_cycle_records([math.radians(47.0)] * 3) @@ -1020,3 +2407,1005 @@ def test_provisional_fit_uses_active_and_passive_axis_model_limits() -> None: ("thumb_mcp", 2.5), ("thumb_ip", 7.5), } + + +def _axis_cycle_records( + travel_rad: float, + axis_xyz: list[float], + *, + cycles: int = 3, + offset_xyz: list[float] | None = None, +) -> list[dict]: + """Synthetic sweeps rotating rigidly about an arbitrary axis.""" + commands = list(range(0, 256, 16)) + [255] + axis = np.asarray(axis_xyz, dtype=float) + axis = axis / np.linalg.norm(axis) + reference = ( + np.array([0.0, 0.0, 1.0]) + if abs(float(axis @ np.array([0.0, 0.0, 1.0]))) < 0.9 + else np.array([0.0, 1.0, 0.0]) + ) + u = np.cross(reference, axis) + u = u / np.linalg.norm(u) + v = np.cross(axis, u) + offset = ( + np.zeros(3) if offset_xyz is None else np.asarray(offset_xyz, float) + ) + records = [] + for cycle in range(cycles): + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): + for command in commands: + angle = travel_rad * (255.0 - command) / 255.0 + rotation = Rotation.from_rotvec(axis * angle) + centre = ( + offset + + 0.03 * (u * math.cos(angle) + v * math.sin(angle)) + ) + records.append( + { + "cycle": cycle, + "direction": direction, + "command_u8": command, + "relative_translation_xyz_m": centre.tolist(), + "image_relative_xy_px": [ + 100.0 * math.cos(angle), + 100.0 * math.sin(angle), + ], + "relative_quaternion_xyzw": ( + rotation.as_quat().tolist() + ), + "parent_pose_common": { + "translation_xyz_m": [0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + }, + "child_pose_common": { + "translation_xyz_m": centre.tolist(), + "quaternion_xyzw": ( + rotation.as_quat().tolist() + ), + }, + "state_u8": [255.0] * 9 + + [float(command)] + + [255.0] * 10, + } + ) + return records + + +def _cross_view_roll_node( + tmp_path, side_tilt_deg: float, side_offset_m: list[float] | None = None +) -> SimpleNamespace: + travel = math.radians(25.0) + front = _axis_cycle_records(travel, [0.0, 0.0, 1.0]) + tilt = math.radians(side_tilt_deg) + side = _axis_cycle_records( + travel, + [math.sin(tilt), 0.0, math.cos(tilt)], + offset_xyz=side_offset_m, + ) + node = _fit_check_node( + {"pinky_mcp_roll": front, "pinky_mcp_roll_side": side} + ) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = { + "pinky_mcp_roll": front, + "pinky_mcp_roll_side": side, + } + node.raw_path = tmp_path / "raw_samples.jsonl" + return node + + +def test_cross_view_roll_axis_disagreement_skips_fusion(tmp_path) -> None: + node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) + + measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) + + # 11.4 deg disagreement was the stable signature of the side-view IPPE + # 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" + 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 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 + + +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. + node = _cross_view_roll_node( + tmp_path, side_tilt_deg=11.0, side_offset_m=[0.021, 0.0, 0.0] + ) + + measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) + + assert measurement.axis_direction_source != "cross_view_weighted_fusion" + diagnostics = [ + json.loads(line) + 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 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 + + +def test_cross_view_roll_diagnostic_uses_real_node_logger(tmp_path) -> None: + # The live node exposes get_logger as a method returning the logger; + # recording the diagnostic must call it instead of treating the bound + # method itself as the logger (regression: session 20260820_133434 + # turned this AttributeError into four axis_fit failures). + node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) + warnings: list[str] = [] + node.get_logger = lambda: SimpleNamespace(warning=warnings.append) + + measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) + + assert measurement.axis_direction_source != "cross_view_weighted_fusion" + assert any("disagree" in message for message in warnings) + + +def test_cross_view_roll_axis_gross_disagreement_still_fails(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) + + +def test_cross_view_roll_axis_agreement_still_fuses(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" + + +def test_side_alias_branch_gap_range_uses_relaxed_limit( + 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 + monkeypatch.setattr( + "g20_thumb_apriltag_calibration.three_camera_node" + ".baseline_hysteresis_by_cycle_rad", + lambda records, zero_command_u8, axis_xyz: [ + math.radians(value) for value in gaps_deg + ], + ) + multiview = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + + def node_for(joints): + node = _fit_check_node({name: records for name in joints}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {name: records for name in joints} + node.raw_path = tmp_path / f"raw_{joints[0]}.jsonl" + return node + + alias_failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node_for(("pinky_mcp_roll_side",)), + replace(multiview, joints=("pinky_mcp_roll_side",)), + ) + assert not [ + failure + for failure in alias_failures + if failure["metric"] == "baseline_directional_gap_range_deg" + ] + + canonical_failures = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + node_for(("pinky_mcp_roll",)), + replace(multiview, joints=("pinky_mcp_roll",)), + ) + ) + range_failure = next( + failure + for failure in canonical_failures + if failure["metric"] == "baseline_directional_gap_range_deg" + ) + assert range_failure["limit"] == 0.3 + + +def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None: + records = _image_cycle_records([math.radians(47.0)] * 3) + multiview = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_roll_multiview" + ) + + def node_for(joints): + node = _fit_check_node({name: records for name in joints}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {name: records for name in joints} + node.raw_path = tmp_path / f"raw_pose_{joints[0]}.jsonl" + wrapped = node._fit_axis_measurement + + def inflated(name, cycle): + measurement = wrapped(name, cycle) + if name in joints: + return replace(measurement, pose_axis_line_rms_m=0.002) + return measurement + + node._fit_axis_measurement = inflated + return node + + alias_failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node_for(("pinky_mcp_roll_side",)), + replace(multiview, joints=("pinky_mcp_roll_side",)), + ) + assert not [ + failure + for failure in alias_failures + if failure["metric"] == "axis_pose_line_rms_mm" + ] + + canonical_failures = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + node_for(("pinky_mcp_roll",)), + replace(multiview, joints=("pinky_mcp_roll",)), + ) + ) + assert [ + failure + for failure in canonical_failures + if failure["metric"] == "axis_pose_line_rms_mm" + ] + + +def _ring_group_end_state() -> list[float]: + """Ring-group avoidance pose: pinky flexed, index/middle rolls parked.""" + state = [float(value) for value in THREE_CAMERA_BASELINE_COMMAND] + state[4] = 0.0 # pinky MCP pitch flexed + state[6] = 254.0 # index roll parked (feedback one count low) + state[7] = 253.0 # middle roll parked + state[19] = 0.0 # pinky PIP flexed + return state + + +def _cross_group_node() -> SimpleNamespace: + middle_roll = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "middle_roll_multiview" + ) + return SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + baseline_command=list(THREE_CAMERA_BASELINE_COMMAND), + sweep_items=[SweepItem(middle_roll, 0, DIRECTION_DECREASING)], + sweep_index=0, + retry_sweep_items=[], + latest_state_u8=_ring_group_end_state(), + endpoint_tolerance_u8=2.0, + ) + + +def test_cross_group_return_keeps_next_group_clearance_motors() -> None: + node = _cross_group_node() + + target = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "next_sweep" + ) + + target_list = list(target) + assert target_list[4] == 0 + assert target_list[19] == 0 + assert target_list[6] == 255 + assert target_list[7] == 255 + # Motors the next group does not need still return to baseline. + assert target_list[8] == THREE_CAMERA_BASELINE_COMMAND[8] + assert target_list[3] == THREE_CAMERA_BASELINE_COMMAND[3] + assert target_list[18] == THREE_CAMERA_BASELINE_COMMAND[18] + + +def test_cross_group_transition_has_no_unfold_refold_redundancy() -> None: + node = _cross_group_node() + middle_roll = node.sweep_items[0].spec + + def moved_motors(waypoints, start): + moved: set[int] = set() + previous = list(start) + for waypoint in waypoints: + moved.update( + index + for index, (left, right) in enumerate(zip(previous, waypoint)) + if left != right + ) + previous = list(waypoint) + return moved + + for parallel in (True, False): + target = ( + G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "next_sweep" + ) + ) + returns = build_calibration_return_waypoints( + target, + current_command=node.latest_state_u8, + profile=RIGHT_19_HAND_PROFILE, + parallel=parallel, + ) + after_return = list(returns[-1]) if returns else node.latest_state_u8 + preparations = build_calibration_preparation_waypoints( + middle_roll, + 255, + current_command=after_return, + baseline=THREE_CAMERA_BASELINE_COMMAND, + profile=RIGHT_19_HAND_PROFILE, + parallel=parallel, + ) + return_moved = moved_motors(returns, node.latest_state_u8) + prep_moved = moved_motors(preparations, after_return) + assert not (return_moved & prep_moved), ( + f"parallel={parallel}: motors {sorted(return_moved & prep_moved)} " + "unfold to baseline and immediately re-flex" + ) + # The pinky stays flexed throughout the whole finger change. + assert 4 not in return_moved and 19 not in return_moved + assert 4 not in prep_moved and 19 not in prep_moved + + +def test_cross_group_return_falls_back_to_baseline_for_thumb() -> None: + thumb = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.motor_index == 0 + ) + node = _cross_group_node() + node.sweep_items = [SweepItem(thumb, 0, DIRECTION_DECREASING)] + + target = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "next_sweep" + ) + + assert list(target) == list(THREE_CAMERA_BASELINE_COMMAND) + + +def _task_validity_node(task_valid: int, task_total: int, window: float): + spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) + records = _image_cycle_records([math.radians(47.0)] * 3) + node = _fit_check_node({"thumb_cmc_pitch": records}) + node.minimum_detection_rate = 0.95 + node.views = { + "front": SimpleNamespace( + valid_rate=window, + task_valid_frames=task_valid, + task_total_frames=task_total, + ) + } + return node, spec + + +def test_task_validity_gate_ignores_post_task_window_pollution() -> None: + # Session 20260820_133904: the sweeps themselves were ~100% valid + # (28.9 Hz synchronised frames), but after the last sweep the + # required-role set flipped back to the full preflight set, the rolling + # window restarted on idle frames at 65%, and a healthy task failed. + node, spec = _task_validity_node( + task_valid=2990, task_total=3072, window=0.65 + ) + + 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_fails_on_bad_task_counters() -> None: + node, spec = _task_validity_node( + task_valid=650, task_total=1000, window=0.99 + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + validity_failure = next( + failure + for failure in failures + if failure["metric"] == "tag_valid_rate_percent" + ) + assert validity_failure["actual"] == 65.0 + assert validity_failure["task_valid_frames"] == 650 + assert validity_failure["task_total_frames"] == 1000 + + +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 + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert not [ + failure + for failure in failures + if failure["metric"] == "tag_valid_rate_percent" + ] + + +def _import_revalidation_node(records_by_joint: dict) -> SimpleNamespace: + node = _fit_check_node(records_by_joint) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_15" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = { + name: records for name, records in records_by_joint.items() + } + node.command_records_by_joint = { + name: [ + dict(record) + for record in records + if int(record.get("cycle", 0)) == 0 + ] + for name, records in records_by_joint.items() + } + node.command_maximum_direction_gap_rad = math.radians(2.0) + node.sweep_items = [ + SweepItem(spec, 0, DIRECTION_DECREASING) + for spec in RIGHT_19_HAND_PROFILE.sweep_specs + ] + return node + + +def test_import_revalidation_drops_warning_band_task_and_prefix() -> None: + # thumb_ip hysteresis 2.18 deg provisionally passed in its source + # session (warning band) but fails the final hard gate; importing it + # anyway made session 20260820_133904 go back to the thumb after the + # four fingers were finished. The failing prefix task and everything + # behind it must be dropped at import time with records cleared. + bad = _image_cycle_records( + [math.radians(47.0), math.radians(47.2), math.radians(55.0)] + ) + good = _image_cycle_records([math.radians(47.0)] * 3) + node = _import_revalidation_node( + {"thumb_cmc_pitch": bad, "thumb_cmc_roll": good} + ) + + accepted, dropped = ( + G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + node, + ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], + ) + ) + + assert accepted == [] + assert dropped[0]["task"] == "thumb_cmc_pitch_front" + assert dropped[0]["failures"][0]["metric"] == "cycle_travel_range_deg" + assert not node.records_by_joint["thumb_cmc_pitch"] + assert not node.records_by_joint["thumb_cmc_roll"] + + +def test_sparse_import_revalidation_keeps_clean_later_task() -> None: + bad = _image_cycle_records( + [math.radians(47.0), math.radians(47.2), math.radians(55.0)] + ) + good = _image_cycle_records([math.radians(47.0)] * 3) + node = _import_revalidation_node( + {"thumb_cmc_pitch": bad, "thumb_cmc_roll": good} + ) + + accepted, dropped = ( + G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + node, + ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], + allow_sparse=True, + ) + ) + + assert accepted == ["thumb_cmc_roll_front"] + assert [item["task"] for item in dropped] == [ + "thumb_cmc_pitch_front" + ] + assert not node.records_by_joint["thumb_cmc_pitch"] + assert node.records_by_joint["thumb_cmc_roll"] + + +def test_sparse_resume_queue_skips_revalidated_later_tasks() -> None: + specs = RIGHT_19_HAND_PROFILE.sweep_specs[:3] + items = [ + SweepItem(spec, cycle, direction) + for spec in specs + for cycle in range(4) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + ] + node = SimpleNamespace( + sweep_items=items, + sweep_index=0, + resumed_task_keys=(specs[0].key, specs[2].key), + ) + + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(node) + assert node.sweep_items[node.sweep_index].spec == specs[1] + + node.sweep_index += 8 + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(node) + assert node.sweep_index == len(items) + + +def test_provisional_gate_catches_final_command_direction_gap() -> None: + records = _image_cycle_records([math.radians(47.0)] * 4) + command_records = [ + dict(record) for record in records if int(record["cycle"]) == 0 + ] + for record in command_records: + if record["direction"] != DIRECTION_INCREASING: + continue + rotation = Rotation.from_quat(record["relative_quaternion_xyzw"]) + command = float(record["command_u8"]) + shift = math.radians(3.0) * (255.0 - command) / 255.0 + shifted = Rotation.from_rotvec([0.0, 0.0, shift]) * rotation + record["relative_quaternion_xyzw"] = shifted.as_quat().tolist() + record["child_pose_common"] = dict(record["child_pose_common"]) + record["child_pose_common"]["quaternion_xyzw"] = ( + shifted.as_quat().tolist() + ) + node = _fit_check_node({"thumb_cmc_yaw": records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", RIGHT_19_HAND_PROFILE.layout_id + ) + node.repetitions = 4 + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {"thumb_cmc_yaw": records} + node.command_records_by_joint = {"thumb_cmc_yaw": command_records} + node.command_maximum_direction_gap_rad = math.radians(2.0) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_yaw_top" + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + command_failure = next( + item + for item in failures + if item["metric"] == "feedback_direction_gap_deg" + ) + assert command_failure["actual"] == pytest.approx(3.0, abs=0.05) + assert command_failure["limit"] == 2.0 + + +def test_product_gate_separates_firmware_deadband_from_backlash() -> None: + records = _image_cycle_records([math.radians(47.0)] * 4) + command_records = _image_cycle_records([math.radians(120.0)]) + for record in command_records: + requested = int(record["command_u8"]) + if requested in {0, 255}: + feedback = requested + elif record["direction"] == DIRECTION_DECREASING: + feedback = min(254, requested + 3) + else: + feedback = max(1, requested - 3) + angle = math.radians(120.0) * (255.0 - feedback) / 255.0 + rotation = Rotation.from_rotvec([0.0, 0.0, angle]) + record["requested_command_u8"] = requested + record["feedback_u8"] = float(feedback) + record["relative_quaternion_xyzw"] = rotation.as_quat().tolist() + record["child_pose_common"] = dict(record["child_pose_common"]) + record["child_pose_common"]["quaternion_xyzw"] = ( + rotation.as_quat().tolist() + ) + node = _fit_check_node({"thumb_cmc_yaw": records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", RIGHT_19_HAND_PROFILE.layout_id + ) + node.repetitions = 4 + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {"thumb_cmc_yaw": records} + node.command_records_by_joint = { + "thumb_cmc_yaw": command_records + } + node.command_maximum_direction_gap_rad = math.radians(2.0) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_yaw_top" + ) + + requested_fit = G20ThreeCameraCalibrationNode._fit_joint_records( + node, "thumb_cmc_yaw", command_records + ) + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + assert math.degrees(requested_fit.maximum_hysteresis_rad) > 2.0 + assert not [ + item + for item in failures + if item["metric"] == "feedback_direction_gap_deg" + ] + + +def test_product_gate_uses_settled_direction_gap_not_dynamic_sweep_lag() -> None: + records = _image_cycle_records([math.radians(70.0)] * 4) + for record in records: + if record["direction"] != DIRECTION_INCREASING: + continue + command = float(record["command_u8"]) + lag = math.radians(3.0) * (255.0 - command) / 255.0 + rotation = Rotation.from_quat(record["relative_quaternion_xyzw"]) + shifted = Rotation.from_rotvec([0.0, 0.0, lag]) * rotation + record["relative_quaternion_xyzw"] = shifted.as_quat().tolist() + record["child_pose_common"] = dict(record["child_pose_common"]) + record["child_pose_common"]["quaternion_xyzw"] = ( + shifted.as_quat().tolist() + ) + settled = _image_cycle_records([math.radians(70.0)])[0:] + node = _fit_check_node({"thumb_mcp": records}) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", RIGHT_19_HAND_PROFILE.layout_id + ) + node.repetitions = 4 + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = {"thumb_mcp": records} + node.command_records_by_joint = {"thumb_mcp": settled} + node.command_maximum_direction_gap_rad = math.radians(2.0) + node.maximum_hysteresis_rad = math.radians(2.0) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_mcp_ip_front" + ) + + dynamic_fit = G20ThreeCameraCalibrationNode._fit_joint_records( + node, "thumb_mcp", records + ) + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, replace(spec, joints=("thumb_mcp",)), + include_view_validity=False, + ) + + assert math.degrees(dynamic_fit.maximum_hysteresis_rad) > 2.5 + assert not [ + item + for item in failures + if item["metric"] + in {"hysteresis_deg", "command_direction_gap_deg"} + ] + + +def test_import_revalidation_accepts_clean_prefix() -> None: + records = _image_cycle_records([math.radians(47.0)] * 3) + node = _import_revalidation_node( + {"thumb_cmc_pitch": records, "thumb_cmc_roll": records} + ) + + accepted, dropped = ( + G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + node, + ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], + ) + ) + + assert accepted == ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"] + assert dropped == [] + assert len(node.records_by_joint["thumb_cmc_pitch"]) == len(records) + + +def test_provisional_fit_can_skip_view_validity_for_imports() -> None: + records = _image_cycle_records([math.radians(47.0)] * 3) + node = _import_revalidation_node({"thumb_cmc_pitch": records}) + node.minimum_detection_rate = 0.95 + node.views = { + "front": SimpleNamespace( + valid_rate=0.0, task_valid_frames=0, task_total_frames=0 + ) + } + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_pitch_front" + ) + + without_views = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + ) + with_views = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=True + ) + ) + + assert not [ + failure + for failure in without_views + if failure["metric"] == "tag_valid_rate_percent" + ] + assert [ + failure + for failure in with_views + if failure["metric"] == "tag_valid_rate_percent" + ] + + +def _warning_band_node(tmp_path, attempts: dict) -> SimpleNamespace: + calls: list[str] = [] + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_pitch_front" + ) + node = SimpleNamespace( + cross_view_roll_diagnostic_finger="", + raw_path=tmp_path / "raw.jsonl", + provisional_warning_ratio=1.25, + sweep_attempts=attempts, + retry_resume_index=None, + retry_sweep_spec=None, + retry_cycles=set(), + repetitions=4, + fit_failure={}, + sweep_index=3, + automatic_fit_retry_limit=2, + paused_reason="", + reason="", + _prepare_failed_sweep_retry=lambda: calls.append("prepare_retry"), + _begin_return_baseline=lambda after: calls.append(f"return_{after}"), + _pause=lambda reason: calls.append(f"pause_{reason}"), + _calls=calls, + _spec=spec, + ) + return node + + +def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None: + node = _warning_band_node(tmp_path, attempts={}) + failures = [ + { + "joint": "thumb_cmc_pitch", + "metric": "axis_cycle_difference_deg", + "actual": 0.779, + "limit": 0.75, + "comparison": "maximum", + } + ] + + paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, node._spec, failures, allow_warning=True + ) + + # The final fit rejects 0.779 against 0.75 on the same records, so the + # first in-band result must rescan here instead of deferring the + # rejection to the end of the session. + assert paused is True + assert "prepare_retry" in node._calls + rows = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + kinds = {row["kind"] for row in rows} + assert "provisional_fit_warning_rescan" in kinds + assert "provisional_fit_warning" not in kinds + assert "fit_failure" in kinds + + +def test_second_warning_band_result_retries_immediately(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_pitch_front" + ) + node = _warning_band_node( + tmp_path, attempts={spec.key: 2} + ) + failures = [ + { + "joint": "thumb_cmc_pitch", + "metric": "axis_cycle_difference_deg", + "actual": 0.779, + "limit": 0.75, + "comparison": "maximum", + } + ] + + paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, node._spec, failures, allow_warning=True + ) + + assert paused is True + assert node._calls == ["prepare_retry", "return_resume_sweep"] + rows = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert [row["kind"] for row in rows] == [ + "provisional_fit_warning_rescan", + "fit_failure", + ] + assert rows[0]["attempt"] == 2 + assert rows[0]["attempt_limit"] == 3 + + +def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_pitch_front" + ) + node = _warning_band_node(tmp_path, attempts={spec.key: 3}) + failures = [ + { + "joint": "thumb_cmc_pitch", + "metric": "axis_cycle_difference_deg", + "actual": 0.751, + "limit": 0.75, + "comparison": "maximum", + } + ] + + paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, node._spec, failures, allow_warning=True + ) + + assert paused is True + assert node._calls == ["pause_joint_fit_check_failed"] + rows = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert [row["kind"] for row in rows] == [ + "provisional_fit_warning_retry_exhausted", + "fit_failure", + ] + assert rows[0]["attempt"] == 3 + assert rows[0]["attempt_limit"] == 3 + + +def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> None: + node = _warning_band_node(tmp_path, attempts={}) + + def failures(travels, orthogonal): + return [ + { + "joint": "thumb_mcp", + "metric": "rotation_orthogonal_rms_deg", + "actual": orthogonal, + "limit": 2.5, + "comparison": "maximum", + }, + { + "joint": "thumb_ip", + "metric": "cycle_travel_range_deg", + "actual": max(travels) - min(travels), + "limit": 10.0, + "comparison": "maximum", + "cycle_travel_deg": travels, + }, + ] + + paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, + node._spec, + failures([74.52, 88.36, 74.58, 74.51], 4.04), + allow_warning=True, + ) + assert paused is True + assert node._calls == ["prepare_retry", "return_resume_sweep"] + + node._calls.clear() + node.sweep_attempts[node._spec.key] = 2 + paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, + node._spec, + failures([74.43, 88.26, 74.54, 74.45], 3.92), + allow_warning=True, + ) + + assert paused is True + assert node._calls == ["pause_joint_fit_repeated_branch_failure"] + assert node.fit_failure["recoverable_by_rescan"] is False + assert node.fit_failure["directions_to_rescan"] == 0 + assert node.fit_failure["repeated_branch_clusters"] is True + + +def test_imported_task_without_capture_skips_view_rate_gate() -> None: + # A fully resumed session (16/16 imported, no live sweeps) has no + # task-scoped view counters; the rolling window then holds idle + # preflight frames judged against the full role set and would fail + # every task's validity gate at the final fit. + node, spec = _task_validity_node( + task_valid=0, task_total=0, window=0.65 + ) + node.resumed_task_keys = (spec.key,) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert not [ + failure + for failure in failures + if failure["metric"] == "tag_valid_rate_percent" + ] + + +def test_live_task_still_uses_window_when_not_resumed() -> None: + node, spec = _task_validity_node( + task_valid=0, task_total=0, window=0.65 + ) + node.resumed_task_keys = () + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + assert [ + failure + for failure in failures + if failure["metric"] == "tag_valid_rate_percent" + ] + + +def test_cross_view_skip_keeps_front_direction_with_side_line(tmp_path) -> None: + # The front roll-link centre trajectory carries the ~21 mm screw + # translation, so on a fusion skip the axis LINE must come from the + # side PIP-link circle while the direction stays front-only. + node = _cross_view_roll_node( + tmp_path, + side_tilt_deg=11.0, + side_offset_m=[0.021, 0.0, 0.0], + ) + + measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) + + assert measurement.axis_direction_source != "cross_view_weighted_fusion" + assert measurement.axis_point_source == "side_circle_cross_view" + side_point = np.asarray( + node.records_by_joint["pinky_mcp_roll_side"][0][ + "parent_pose_common" + ]["translation_xyz_m"], + dtype=float, + ) + measured_point = np.asarray( + measurement.point_common_xyz_m, dtype=float + ) + front_direction = np.asarray( + measurement.axis_common_xyz, dtype=float + ) + # The published line keeps the front direction and takes the side + # view's line position (offset ~21 mm by construction), instead of the + # front link's own displaced line. + assert float(np.dot(front_direction, [0.0, 0.0, 1.0])) > math.cos( + math.radians(2.0) + ) + assert 0.015 < float(measured_point[0]) < 0.027 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 e44bc79..e616820 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -19,6 +19,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import ( UrdfKinematicModel, _angles_from_state, _zero_sensitive_axis_error_rad, + baseline_hysteresis_by_cycle_rad, fit_joint_axis_measurement, fit_rotation_joint_curve, solve_urdf_zero_offsets, @@ -157,6 +158,78 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None: assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6) +def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None: + off_axis_noise = Rotation.from_rotvec( + np.radians([1.2, 0.0, 0.2]) + ).as_quat().tolist() + records = [] + for cycle in range(3): + records.extend( + ( + { + "cycle": cycle, + "direction": "decreasing", + "command_u8": 255, + "relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + }, + { + "cycle": cycle, + "direction": "increasing", + "command_u8": 255, + "relative_quaternion_xyzw": off_axis_noise, + }, + ) + ) + + full_pose = baseline_hysteresis_by_cycle_rad( + records, zero_command_u8=255 + ) + joint_angle = baseline_hysteresis_by_cycle_rad( + records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0] + ) + + assert math.degrees(full_pose[0]) == pytest.approx( + math.hypot(1.2, 0.2), abs=1.0e-9 + ) + assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9) + + +def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None: + records = [] + commands = sorted({0, 127, 255, *range(0, 256, 16)}) + branch_offset = math.radians(1.0) + for cycle in range(3): + for direction in ("decreasing", "increasing"): + offset = branch_offset if direction == "increasing" else 0.0 + for command in commands: + angle = math.radians(50.0) * (127.0 - command) / 255.0 + angle += offset + rotation = Rotation.from_rotvec([0.0, 0.0, angle]) + records.append( + { + "cycle": cycle, + "direction": direction, + "command_u8": command, + "relative_quaternion_xyzw": rotation.as_quat().tolist(), + } + ) + + curve = fit_rotation_joint_curve( + records, + zero_command_u8=127, + canonical_zero_direction="decreasing", + ) + + assert curve.angle_rad == curve.decreasing_rad + assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9) + assert curve.increasing_rad[127] == pytest.approx( + branch_offset, abs=1.0e-6 + ) + assert curve.maximum_hysteresis_rad == pytest.approx( + branch_offset, abs=1.0e-6 + ) + + @pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"]) def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth( joint: str, @@ -234,6 +307,35 @@ def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None: ) < 1.0e-6 +def test_side_roll_axis_point_ignores_real_screw_translation() -> None: + records, expected_axis, expected_point = _arbitrary_tag_records() + common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) + axis_parent = common_from_parent.inv().apply(expected_axis) + screw_records = [] + for record in records: + changed = dict(record) + fraction = (255.0 - float(record["command_u8"])) / 255.0 + changed["relative_translation_xyz_m"] = ( + np.asarray(record["relative_translation_xyz_m"], dtype=float) + + 0.008 * fraction * axis_parent + ).tolist() + screw_records.append(changed) + + measurement = fit_joint_axis_measurement( + "middle_mcp_roll_side", + screw_records, + cycle=0, + zero_command_u8=255, + constrained_circle_joints=frozenset({"middle_mcp_roll_side"}), + ) + + observed_point = np.asarray(measurement.point_common_xyz_m) + assert measurement.pose_axis_line_rms_m < 1.0e-6 + assert np.linalg.norm( + np.cross(observed_point - expected_point, expected_axis) + ) < 1.0e-6 + + def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None: records, expected_axis, _ = _arbitrary_tag_records() assert not any(record["command_u8"] == 127 for record in records) @@ -479,15 +581,17 @@ def _solve_synthetic_offsets( side: str, offset_degrees: list[float], *, + layout_id: str = "legacy_11", inject_oblique_optical_depth_bias: bool = False, inject_secondary_root_axis_bias_degrees: float = 0.0, inject_secondary_root_point_bias_m: float = 0.0, inject_observer_cone_bias_degrees: float = 0.0, pose_axis_line_rms_by_joint_m: dict[str, float] | None = None, joint_maximum_offset_degrees: dict[str, float] | None = None, + validation_offset_bias_degrees: dict[str, float] | None = None, ): - hand = get_hand_calibration_profile(side) - zero = get_zero_calibration_profile(side) + hand = get_hand_calibration_profile(side, layout_id) + zero = get_zero_calibration_profile(side, layout_id) source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF baseline = [255.0] * 20 baseline[6:10] = [127.0] * 4 @@ -517,7 +621,9 @@ def _solve_synthetic_offsets( base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) base_translation = np.asarray([0.31, -0.19, 0.72]) measurements: list[JointAxisMeasurement] = [] - for cycle in range(3): + validation_cycle = 3 if layout_id == "g20_right_15" else 2 + training_cycles = tuple(range(validation_cycle)) + for cycle in range(validation_cycle + 1): for joint in zero.axis_joints: state = list(baseline) if joint == "thumb_cmc_yaw": @@ -528,8 +634,18 @@ 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=offsets, joint_angles=angles + joint, zero_offsets=cycle_offsets, joint_angles=angles ) point_common = base_rotation.apply(point) + base_translation if ( @@ -622,14 +738,229 @@ def _solve_synthetic_offsets( curves=curves, motor_by_joint=motor_by_joint, hand_type=side, + tag_layout=layout_id, joint_maximum_offset_rad={ name: math.radians(value) for name, value in (joint_maximum_offset_degrees or {}).items() }, + training_cycles=training_cycles, + validation_cycle=validation_cycle, ) return zero, result +def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None: + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, + 0.8, -0.7, + -0.5, 0.6, + 1.1, -1.0, + ] + zero, result = _solve_synthetic_offsets( + "right", injected, layout_id="g20_right_15" + ) + + assert len(zero.direct_zero_joints) == 12 + assert len(zero.axis_joints) == 17 + assert result.passed is True + 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 + ] + ) + ) + for name, expected in zip(zero.direct_zero_joints, injected): + if name == "thumb_mcp": + expected = 0.0 + elif name in finger_rolls: + expected -= roll_common + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + expected, abs=0.05 + ) + assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx( + 0.0, abs=0.05 + ) + assert result.training_cycles == (0, 1, 2) + assert result.validation_cycle == 3 + assert all( + value == pytest.approx(0.0, abs=1.0e-12) + for value in result.offset_confidence_half_width_rad.values() + ) + assert all( + result.all_active_offsets_rad[f"{finger}_pip"] == 0.0 + for finger in ("index", "middle", "ring", "pinky") + ) + assert result.observability_parameter_count == 18 + assert result.observability_rank == 18 + assert math.isfinite(result.observability_condition_number) + assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) + + +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. + injected = [ + 2.0, -3.0, 4.0, -1.5, + 6.0, -1.0, + 6.4, -0.7, + 5.7, 0.6, + 6.2, -1.0, + ] + + zero, result = _solve_synthetic_offsets( + "right", injected, layout_id="g20_right_15" + ) + + assert result.passed is True + 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 + ] + ) + ) + for name, expected in zip(zero.direct_zero_joints, injected): + if name == "thumb_mcp": + expected = 0.0 + elif name in finger_rolls: + expected -= roll_common + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + expected, abs=0.05 + ) + assert np.median( + [result.direct_offsets_rad[name] for name in finger_rolls] + ) == pytest.approx(0.0, abs=1.0e-10) + + +def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, + 0.8, -0.7, + -0.5, 0.6, + 1.1, -1.0, + ] + zero, result = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_15", + validation_offset_bias_degrees={"thumb_cmc_roll": 2.0}, + ) + + assert math.degrees( + result.direct_offsets_rad["thumb_cmc_roll"] + ) == pytest.approx(injected[0], abs=0.05) + assert result.validation_cycle not in result.training_cycles + + +def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> None: + """Guard the reviewed 6-palm-DOF plus 12-static-zero observability.""" + zero = get_zero_calibration_profile("right", "g20_right_15") + model = UrdfKinematicModel(RIGHT_SOURCE_URDF) + parameter_count = 6 + len(zero.direct_zero_joints) + + def line_observations(parameters: np.ndarray) -> np.ndarray: + palm_rotation = Rotation.from_rotvec(parameters[:3]) + palm_translation = parameters[3:6] + offsets = { + name: float(value) + for name, value in zip( + zero.direct_zero_joints, parameters[6:] + ) + } + result: list[float] = [] + for joint in zero.axis_joints: + axis, point = model.axis_line( + joint, zero_offsets=offsets, joint_angles={} + ) + axis = palm_rotation.apply(axis) + point = palm_rotation.apply(point) + palm_translation + # An oriented 3-D line is represented by its direction and + # Pluecker moment. The moment is invariant to choosing a different + # point along the same axis, so no unobservable along-axis Tag + # placement is accidentally counted as information. + result.extend(float(value) for value in axis) + result.extend(float(value) for value in np.cross(point, axis)) + return np.asarray(result, dtype=float) + + origin = np.zeros(parameter_count, dtype=float) + step = 1.0e-6 + jacobian = np.column_stack( + [ + ( + line_observations( + origin + np.eye(parameter_count, dtype=float)[index] * step + ) + - line_observations( + origin - np.eye(parameter_count, dtype=float)[index] * step + ) + ) + / (2.0 * step) + for index in range(parameter_count) + ] + ) + + assert parameter_count == 18 + assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count + + +def test_right_15_urdf_writer_changes_only_the_12_static_targets( + tmp_path: Path, +) -> None: + zero = get_zero_calibration_profile("right", "g20_right_15") + offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints} + destination = write_zero_corrected_urdf( + source_urdf=RIGHT_SOURCE_URDF, + output_directory=tmp_path, + serial_number="G20_RIGHT_019", + offsets_rad=offsets, + timestamp="20260818_120000", + ) + + original = { + str(joint.get("name")): joint + for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint") + } + corrected = { + str(joint.get("name")): joint + for joint in ET.parse(destination).getroot().findall("joint") + } + for mesh in ET.parse(destination).getroot().findall(".//mesh"): + relative = Path(mesh.get("filename")) + copied = destination.parent / relative + source = RIGHT_SOURCE_URDF.parent / relative + assert copied.is_file() + assert copied.stat().st_size == source.stat().st_size + changed = set() + for name in original: + original_origin = original[name].find("origin") + corrected_origin = corrected[name].find("origin") + if original_origin is None or corrected_origin is None: + continue + if original_origin.get("rpy") != corrected_origin.get("rpy"): + changed.add(name) + assert original_origin.get("xyz") == corrected_origin.get("xyz") + assert changed == set(zero.direct_zero_joints) + assert "thumb_mcp" in changed + assert not set(get_hand_calibration_profile( + "right", "g20_right_15" + ).passive_joints) & changed + + def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None: zero, result = _solve_synthetic_offsets("right", [0.1] * 7) assert result.passed is True @@ -712,7 +1043,7 @@ def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None assert result.passed is True assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0) - assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0)) + assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0)) assert "thumb_ip" not in result.validation_error_by_joint_rad for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"): assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0