diff --git a/config/o30_three_camera_extrinsics_20260920_123651.yaml b/config/o30_three_camera_extrinsics_20260920_123651.yaml new file mode 100644 index 0000000..69c289b --- /dev/null +++ b/config/o30_three_camera_extrinsics_20260920_123651.yaml @@ -0,0 +1,60 @@ +schema_version: 1 +reference_view: front +cameras: + front: + serial_number: DB2163742 + width: 1624 + height: 1240 + intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07 + side: + serial_number: DB2163749 + width: 1624 + height: 1240 + intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655 + top: + serial_number: DB2163739 + width: 1624 + height: 1240 + intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446 +front_from_view: + front: + translation_xyz_m: + - 0.0 + - 0.0 + - 0.0 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + side: + translation_xyz_m: + - -0.8521079207290448 + - 0.0021412076894364238 + - 1.0361400609750895 + quaternion_xyzw: + - -0.04900255095488596 + - 0.676537184995331 + - -0.016467372664989974 + - 0.7345917321587682 + top: + translation_xyz_m: + - 0.0004373257585413154 + - -0.5686488899083978 + - 1.1253160870539256 + quaternion_xyzw: + - 0.7278983925982527 + - 0.060947357492050866 + - 0.04110103817603059 + - -0.6817331254446042 +quality: + passed: true + reprojection_rms_px: 0.9071907304479074 + maximum_rotation_repeatability_deg: 0.05898535309651316 + maximum_translation_repeatability_m: 0.0007701935623164992 + front_side_captures: 15 + front_top_captures: 15 + front_side_candidates: 15 + front_top_candidates: 15 + front_side_rejected: 0 + front_top_rejected: 0 diff --git a/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md b/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md index 6dfb36d..68cd83b 100644 --- a/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md +++ b/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md @@ -2,8 +2,8 @@ 入口为 `calibrate_hand --config src/linkerhand_calibration/config/o30_right_product.yaml`。 Profile 为 `O30/right/o30_right_18/v1`，20 个关节全部主动；原始 URDF 与外部 SDK 不被标定器改写。 -当前产品配置绑定正面相机移动后重新采集的 `config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml`。 -该外参的 SHA256 为 `59f587e8e371f09456043c17cd0268fe1f170f1b7efdf9d2cb744c73c194953c`； +当前产品配置绑定 2026-09-20 重新采集的 `config/o30_three_camera_extrinsics_20260920_123651.yaml`。 +该外参的 SHA256 为 `e21d21eaaf198d4daba44a44e0aa3bc46a86376213b84c3ddd30ae571714c701`； 三相机序列号及内参指纹均与现有文件一致。 软件测试包含合成数据和已记录的真实图像回归，不能作为整手现场精度报告。 @@ -376,13 +376,15 @@ O30 使用 `motion_observation: feedback`、`feedback_travel_matches_command: fa ## 相机移动后重标外参 仅改变相机位置和朝向，镜头、焦距、对焦及成像分辨率/裁剪均未改变时，可以沿用内参。 -这次已沿用 `~/.ros/camera_info/` 内三份文件，并重新求得三相机之间的外参： -重投影 RMS 为 0.933275 px，最大旋转重复性为 0.052949°，最大平移重复性为 0.496086 mm； -FRONT+SIDE、FRONT+TOP 各 15 组有效观测，质量检查通过。以下命令保留供下次移动相机后重新标定。 +2026-09-20 已沿用 `~/.ros/camera_info/` 内三份文件，并重新求得三相机之间的外参： +重投影 RMS 为 0.907191 px，最大旋转重复性为 0.058985°，最大平移重复性为 0.770194 mm； +FRONT+SIDE、FRONT+TOP 各 15 组有效观测，均无剔除，质量检查通过。 +以下命令供下次移动相机后重新标定，输出到新文件以保留历史外参。 以下命令使用当前工具默认的 **8×5 内角点、27 mm 方格**；棋盘不一致时必须修改对应三个参数。 ```bash +EXTRINSICS_FILE="$PWD/config/o30_three_camera_extrinsics_$(date +%Y%m%d_%H%M%S).yaml" ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ front_camera_serial:=DB2163742 \ side_camera_serial:=DB2163749 \ @@ -391,15 +393,15 @@ ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ side_camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163749.yaml \ top_camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163739.yaml \ checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \ - output_file:=$PWD/config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml + output_file:="$EXTRINSICS_FILE" ``` 在界面分别采集 FRONT+SIDE、FRONT+TOP 的不同棋盘姿态，每对至少 15 组有效观测，质量通过后点击 SAVE。 通用外参工具沿用 `/g20_extrinsics/...` 图像命名空间，此名称不会改变输出的相机身份或 O30 绑定。 -退出外参工具，再计算新文件的 SHA256： +退出外参工具，在同一终端计算新文件的 SHA256： ```bash -sha256sum config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml +sha256sum "$EXTRINSICS_FILE" ``` 将输出路径和 SHA256 分别填写到 `src/linkerhand_calibration/config/o30_right_product.yaml` 的 @@ -407,12 +409,24 @@ sha256sum config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml 相机移动前的外参和采集记录仅保留作历史诊断，新外参绑定后应从头采集整手数据。 配置检查会核对三份内参指纹、相机序列号、外参质量和各输入文件 SHA256。 +本机 O30 实机标定使用独立 ROS 域；标定、SDK 子进程及另开的监控终端需使用同一值。 +它隔离其他域的话题发现，不隔离其他进程对同一 CAN 设备的直接访问，仍保留控制发布者检查。 + +分阶段回访/断点零位复核使用 `staged_revisit_v2`：根据本机操作者提供的正常重复到位范围， +O30 跨次反馈与原零位反馈中位数的差值上限为 10 个 SDK 原始值（包含 10）， +同一批采样的反馈极差上限仍为 2。检查仍覆盖整手保持反馈，并同时检查原图像姿态、安装证据 +和新图像时间；不会重新定义冻结零位。该值不是角度精度声明，也不改变运动到位、几何求解、 +角度或像素验收门限。每条新复核记录保存实际门限，离线严格核对；旧 `staged_revisit_v1` +记录继续按原跨次上限 4 回读，其他型号门限保持原值。 + ```bash +export ROS_DOMAIN_ID=99 + ros2 run linkerhand_calibration calibrate_hand --config \ src/linkerhand_calibration/config/o30_right_product.yaml --validate-only ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/o30_right_product.yaml + src/linkerhand_calibration/config/o30_right_product.yaml --no-resume ``` ## 通道、观测与任务 @@ -711,3 +725,161 @@ JSON 重建修正 URDF 的一致性检查。缺失/恒定反馈的旧测试显 不代表当前生产配置允许在缺失反馈时采集。 产品配置只读校验通过，外部 SDK 的 61 份源码和原始 URDF 哈希保持不变。 本次未启动实机；新的现场精度及正式产物仍需实际完整采集与独立验收。 + +## 遮挡条件下的独立姿态准备（2026-09-20，显式启用） + +用户确认四指伸直侧摆时，侧面 ID4/6/8/10 会互相遮挡。因此不能要求四指侧摆的 +同一段运动提供四个侧面 Tag 的证据，也不将 G20 的双视角侧摆验收直接套到 O30。 +O30 正式四指侧摆仍用原正面观测；屈伸仍用原侧面观测。新增策略不修改 Tag、相机、 +正式任务顺序、训练/独立验证网格、速度或发布门限。 + +`occlusion_aware_witness_v1` 为四个 MCP pitch 任务各声明一份准备证据： + +1. 按该任务原有分组进入避让/零位姿态，先完成外侧手指弯曲。 +2. 保持所有其他通道，仅当前手指侧摆通道往返一次，幅度最多为原通道范围的四分之一 + （O30 为 63 个 SDK 原始单位）。回到相同姿态后，再执行原 MCP 屈伸准备。 +3. 侧面同一个物理 Tag 在侧摆弧上的独立图像拟合先通过自己的训练/留出验收， + 全参数协方差包含轴、安装和每帧角度；重复端点图像不降低独立证据要求。 +4. 在新的 MCP 零位端点复核原姿态，用这份证据约束 MCP 候选。MCP 自己的图像、 + 几何及留出门限继续执行。准备路径缺失、参考变化、持续歧义均不得强选或反复试到通过。 + +正面相机可并行保存当前侧摆 Tag 的原始观测，但不作为所有屈伸关节的第二路角度测量， +不要求无关手指的 Tag 同时可见。PIP/DIP 继续复用原父几何/旁观证据链。 +这一策略新增有界准备动作，需要单独验证运动及可见性；总耗时收益必须由实机测量， +不宣称“零额外动作”或已经保证整手连续通过。 + +启动前的纯软件配置检查（不启动相机/SDK）： + +```bash +export ROS_DOMAIN_ID=99 +ros2 run linkerhand_calibration calibrate_hand \ + --config src/linkerhand_calibration/config/o30_right_product.yaml \ + --training-policy staged_2_to_3 \ + --preparation-policy occlusion_aware_witness_v1 \ + --validate-only +``` + +实机验证需全新会话：将 `--validate-only` 换成 `--no-resume`。新策略不会自动启用； +旧断点仍用原参数恢复，不能加此参数拼接旧策略。准备定义进入原 capture_plan_v2 的 +运动契约和哈希；旧计划省略空定义，与 `20260920_144151` 的实际历史计划逐字段一致。 +新策略同策略断点保留原证据和重试边界，重新核查参考后恢复；冻结零位的回访及最终 +独立验证不重复侧摆准备。新会话清空内存证据，历史原始记录不改写。 + +软件验证：真实无名指 MCP 的 127 帧在无新增证据时仍拒绝；在其上加入明确标注的 +合成独立侧摆观测后，正式准备/证据回读入口可判别候选。合成源以候选 0 构造， +不能据此认定真实机械手候选 0 正确。真实中指 DIP 仍拒绝，既有拇指 IP 和姿态转接 +回归通过。最终针对遮挡版的 58 项软件检查约 14.12 秒。既有 O30 分阶段产物一致性 +与取消发布共 2 项共享一次整手夹具，约 32.12 秒；它们不是新准备策略的整手实机验收。 +本轮没有启动硬件，没有发布新的 JSON/URDF。 + +### 准备求解的有界数值恢复 + +`20260920_154722` 调整 ID2 后，零位 ID1/ID2 均已可见，但 IP 的一个候选在稀疏求解 +中耗尽 150 次求值。它不属于缺 Tag 或已经证明的姿态歧义；未收敛也不能据此排除候选。 + +独立铰链、测量迁移、共享零位、姿态转接现在共用 `image_bundle_optimizer`：先按原 +稀疏路径求解，仅在预算耗尽且结果有限时，从当前参数追加一次直接信赖域求解，默认 +最多 50 次。参数、几何边界、角点和收敛容差保持一致；不得根据留出误差决定是否追加。 +直接解法受维度和 Jacobian 大小限制，覆盖默认最多六个关系/72 个训练帧；计算仍受 +既有工作进程的 120 秒超时及取消控制。已经收敛的历史模型不走恢复路径。 + +每个候选报告实际算法、预算、终止状态、总求值数及代价；发生恢复时，策略与过程写入 +原准备记录并绑定证据哈希。恢复后仍需通过原几何可观测性、像素门限和独立留出比较。 +`OBS-SOLVER-118` 区分计算预算不足与 `OBS-POSE-AMBIGUOUS-117` 的真实姿态歧义； +计算失败不触发重复机械运动，不覆盖冻结模型，也不更新发布指针。 + +本次真实 IP 数据恢复后通过软件准备及证据回读，额外直接求解为 6 次、实测约 0.05 秒。 +这是离线计算耗时，不是整手标定提效或实机连续通过的证明；此软件修复轮未启动机械手。 + +### 辅助准备证据的适用性与短暂同步缺失 + +`20260920_160630` 实机恢复已通过拇指 IP：另一候选在原 150 次求值后追加 8 次收敛， +按原像素门限排除；共保留 8 个扫描单元。随后小指 MCP 的 219 帧辅助观测中有 2 帧 +无同步指令，被旧辅助验证误报为采集身份变化。该任务辅助侧摆的两条候选均不满足 +原 15° 最小转角要求；它自己的 MCP 准备运动则能够独立通过全部门限。此前的合成 +辅助观测有足够转角，因此没有暴露真实硬件上的适用性问题。 + +新记录 `preparation_witness_v2` 分开处理原始身份、采样有效性和辅助几何适用性： + +- 对全部原始行检查身份、相机、已有指令和反馈。缺指令的帧若反馈显示保持关节变化， + 仍拒绝；缺反馈的帧也不能隐藏已有指令的变化。非有限值或向量格式错误不作为缺样跳过。 +- 缺同步值的帧不参与拟合，不插值、不补造。比例不得超过既有 `maximum_unobserved_fraction` + （默认 1/16）；必须在同一运动分段内被有效帧包围，间隙不得超过既有稳定窗口 + `steady_window_seconds`（O30 为 0.2 秒）。分段边界和连续稳态末尾仍须有效。 + 相机/状态的原 50 ms 同步门限保持不变。原始行、排除时间戳和原因全部绑定到证据中。 +- 在任何辅助拟合之前，仅当全部原始候选都不满足原最小转角时，明确记录 + `independent_before_fit`。主准备运动随后独立通过全部候选、几何、像素及留出门限。 + 缺候选、身份变化不等于转角不足；一旦辅助拟合开始，失败不得改走独立路线。 +- 采用辅助证据和拟合前判定不适用这两种情况都必须保留辅助记录。离线从完整原始窗口 + 重算排除清单和适用性，再重算主模型。删去记录、丢掉缺样帧、改写排除原因仍拒绝。 + v1 历史证据沿用原严格取样语义，不改写已保存记录。 + +本次小指软件回放经正式准备、证据哈希及原像素重算入口通过；真实无名指歧义仍拒绝。 +运动路径、速度、Tag、训练/验证分组及发布门限没有修改。本次软件修复未启动硬件， +不能据此声明整手连续通过；若后续关节独立运动仍有歧义且辅助观测不适用，仍需停止。 + +## 2026-09-20：MCP → PIP → DIP 的双端姿态交接 + +`20260920_163002` 已实际复用原 8 个扫描单元并完成小指 MCP 双向扫描，随后在 PIP +准备停止。原交接把 PIP 回零后的图像再次投影到旧 MCP 单轴模型上；最后 10 帧中 +4 帧 RMS 为 1.526–1.600 px，触发原 1.5 px 门限。继续保持时仍有类似波动，不能靠 +删去超限帧或原样多等一次来证明通过。原模型误差并不直接证明 Tag 松动。 + +新的 `two_sided_shared_pose_v1` 使用同一物理基准的两端证据： + +- 原 MCP 模型检查 MCP 已有回基准运动中的最后完整原始图像窗口，必须早于 PIP + 整段准备弧。PIP 模型检查 PIP 回基准的独立窗口，并继续通过准备训练/留出验收。 +- 两端都保留 1.5 px、姿态一致性 2°/5 mm、静止散布 1°/1 mm 等原门限。 + 固定窗口按时间选取，不能按误差筛选；命令、反馈、Tag 编号/尺寸、相机、轮次、 + 观察代次及像素哈希共同绑定，验证轮不能作为交接源。 +- 原 MCP 零位及模型不修改。进入 DIP 时，由已经通过上述交接的 PIP 图像模型观测 + 当前父 Tag，但仍与原 MCP 零位比较，不重新解释或替换原零位。记录为 + `parent_reference_verified` v2；几何来源仍是实测 PIP，末节 CAD 假设保持原语义。 +- 实时侧仅保留每任务/机位的有限原始图像尾部，复用已有采集和状态机，不增加动作。 + 断点导入保留交接依赖图像；重复导入同一曝光只计一次，冲突副本拒绝。只有日志记录 + 的内部恢复可跨观察代次继承，重新启动仍需重新核验参考。历史记录按原策略读回。 + +该逻辑用于 `staged_2_to_3` 加 `occlusion_aware_witness_v1` 的准备流程；采集计划、 +速度、避让、Tag 布局及最终发布策略没有变化，未完成任务可从原断点重新进入。 +两端验证在准备求解时执行，不把上游通过等同于整指或整手通过。 + +真实 PIP 回放通过正式准备及证据回读：留出 RMS 0.3584 px，最大帧 RMS 0.7464 px。 +回零姿态差最大约 1.973°，接近现有 2°门限；后续实测超限仍须拒绝。 +DIP 只完成模拟后续访问的正式采集/父参考回读测试，使用真实 PIP 像素和明确标注的 +合成下游模型外壳，不冒充真实 DIP 标定。整手连续通过及最终实测精度仍待实机验收。 + +## 准备采样预算与连续运行（2026-09-20） + +`staged_2_to_3` 配合 `occlusion_aware_witness_v1` 时，单关节准备按 256 个指令区间 +保留观测，每区间最多一帧；训练与独立留出各最多 144 帧。此预算在采集/拟合前确定， +不根据哪一个候选得分较好追加数据。静止重复帧不增加运动区间；四指同时运动仍使用原 +128 区间、72/72 帧预算，保持多关节求解内存上限。运动速度、范围、3°/3 mm 几何门限、 +1.5 px 图像门限、候选区分显著性与最终产物验收均未放宽。 + +原因：`20260920_171710` 的小指 PIP 原准备保留 129 帧，DIP 保留 128 帧；PIP +自身过关，但其不确定度传给 DIP 后，两候选均超过 3°。从同一原始运动窗口保留更完整 +观测后，两关节各得到 248 帧。离线反事实回放还使用新 PIP 模型重新投影原 DIP 图像， +验证整条依赖链；不修改原零位或历史日志，不把旧断点直接重新标记为通过。 + +这项优化需要新采集的上游模型。验证整场稳定性使用 `--no-resume`；继续旧断点仍保留 +原冻结模型及不确定度，不能承诺仅重试 DIP 就会通过。正式运行命令： + +```bash +ros2 run linkerhand_calibration calibrate_hand \ + --config src/linkerhand_calibration/config/o30_right_product.yaml \ + --training-policy staged_2_to_3 \ + --preparation-policy occlusion_aware_witness_v1 --no-resume +``` + +断点初始化现在发布 `PREPARING_CHECKPOINT` 心跳，独立加载上限为 900 秒。设备就绪 +等待和 Start 服务仍使用各自原有时限；心跳不授权运动、不跳过历史图像校验，也不代表 +加载变快。发布线程在正常回调开始前退出，初始化异常时同样清理。 + +软件验证和现场记录位于 +`calibration_output/O30_RIGHT_001/software_review_stability_density/`。 +真实像素回放通过与整手实机连续通过分别记录；合成 JSON→URDF 测试产物不能作为本机标定交付。 + +新准备记录将 `image_sampling_policy: native_command_256_split144_v1` 同时写入初始化 +报告和按角色哈希绑定的证据。辅助运动、独立准备、断点与最终验收的重新拟合均读取 +这项策略；无字段的旧日志继续使用原预算。删除策略、改写策略或使训练/留出帧数超过 +对应预算会拒绝回读，不能让在线与离线分别选择不同的子集。 diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index c20220a..e876153 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -13,6 +13,38 @@ O30 右手的构建、相机外参重标和完整操作说明见 [O30 右手标 O30 当前使用反馈判断运动、稳定及零位恢复；允许指令与反馈存在固定偏差。最终 JSON 仍为指令→视觉角度，反馈映射只作诊断。 测试的分层、选择范围与耗时记录见 [测试说明](TESTING.md)。 +## O30 分阶段训练试验 + +新增显式选项 `--training-policy staged_2_to_3`，默认仍为 `fixed`。Tag 数量、编号、尺寸、位置、 +相机布局、完整网格、运动速度和验收门限不变。新策略按原任务顺序执行全手轮次 0、全手轮次 1， +进行训练检查；必要时仅集中补一次轮次 2，随后冻结整手模型，最后采集独立验证轮次 3。 +局部零位补采包含同轮几何依赖，分段任务整体补采，共享掌部问题扩大到全手。补采后仍不合格即失败。 + +```bash +# 仅校验配置；不连接、启动硬件 +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml \ + --training-policy staged_2_to_3 --validate-only +``` + +准备阶段会保留已有上游运动中可见的相关末节 Tag 原始角点及合法候选；缺失旁观 Tag 不阻塞 +上游任务。末节保持指令、反馈、参考和独立图像检查全部合格后，测得的相对位姿才能约束后续准备。 +上游图像及几何不确定度进入验收；原有图像无法区分的姿态仍拒绝。旧日志不会补造缺失角点。 + +回访复核原零位后复用原模型。训练进程、任务索引和局部拟合在会话内复用；冻结文件 +`frozen_training.json` 绑定配置、源 URDF、计划、输入证据和求解版本。在线最终验收不重新训练； +离线审计仍复算决定。新策略断点仅复用完整任务访问，重试额度跨断点保留，冻结后不回退到训练。 +同一会话的 `raw_samples.jsonl` 和 `frozen_training.json` 应一起保留，离线回放须指定相同策略。 + +两轮全部通过时，扫描单元从 144 减到 108、正式稳态停点从 1390 减到 1052；这不是总耗时降幅。 +阶段重排增加回访和参考复核，实机对照须计入这些时间。`stage_timing.json` 区分阶段耗时、开始到 +终态耗时及开始前断点加载耗时，终态后等待退出不计入开始到终态指标。独立验证失败不得回炉训练。 +JSON→URDF 重建、文件与像素验收、成对发布、失败不更新通过指针，以及五个末节 CAD 零位声明均保留。 + +软件检查与实机验收分开记录。完整实机通过、人工干预不增加且同条件总耗时至少降低 10% 后， +才能评估切换默认策略；本次软件实现不自动启动硬件。实现与验收说明见 +[分阶段标定实施记录](STAGED_CALIBRATION_IMPLEMENTATION.md)。 + ## O30 自适应训练试验 默认仍为 `fixed`：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后 @@ -32,10 +64,10 @@ ros2 run linkerhand_calibration calibrate_hand --config \ --training-policy adaptive_2_to_3 --no-resume ``` -当前 O30 掌部共享几何依赖最后采集的四指侧摆。前面的任务无法可靠提前计算零位置信区间， +旧 `adaptive_2_to_3` 顺序中，O30 掌部共享几何依赖最后采集的四指侧摆。前面的任务无法可靠提前计算零位置信区间， 因此保守保留第三轮；最后一个任务满足全部门限时才减轮。本版完整计划的稳态停点最多从 1390 降到 1340（约 3.6%），不承诺总耗时同比下降。所有任务都省一轮时的 1052 个停点只是 -理论下界，当前依赖和运动顺序下不能实现。实机连续通过及实际耗时验收完成前，不启用新默认。 +理论下界，该旧顺序下不能实现。上面的分阶段策略在两轮全手证据齐全后统一判断，不受此逐任务调度限制。 采集计划 `capture_plan_v1` 绑定任务、运动分段、准备路径、网格与实际训练/验证身份。 轮次 ID 保持不变：减轮后依次采集 0、1、3，界面显示第 1、2、3 轮；ID 3 始终为独立验证。 diff --git a/src/linkerhand_calibration/STAGED_CALIBRATION_IMPLEMENTATION.md b/src/linkerhand_calibration/STAGED_CALIBRATION_IMPLEMENTATION.md new file mode 100644 index 0000000..3cc7b6f --- /dev/null +++ b/src/linkerhand_calibration/STAGED_CALIBRATION_IMPLEMENTATION.md @@ -0,0 +1,92 @@ +# 固定 Tag 的分阶段标定实施记录 + +默认 `fixed` 保留。新策略通过 `--training-policy staged_2_to_3` 显式启用；当前满足其 +“实测指令发布、完整交错网格”能力约束的内置配置为 O30。通用实现不按手型号或相机名称分支。 +本次没有启动硬件、修改 Tag/相机配置或更新已发布产物。 + +## 执行和证据 + +- `capture_plan_v2` 描述各阶段、原任务顺序及实际训练轮。引擎按轮次排列现有扫描单元， + 继续使用原基准、避让、进入/退出路径。首轮数据直接用于正式训练。 +- 两轮全手数据齐全后检查曲线重复性、空间几何、零位置信度和 Tag 安装。训练问题使用 + 参数、原因、范围和任务依赖描述；局部零位缺陷补齐同轮依赖，共享根部几何缺陷扩大到全手。 +- 第三轮只允许一个集中补采批次，轮次固定为 2。仍不合格即失败。独立验证始终是轮次 3。 +- 每次回访保存新零位复核记录，原零位、安装模型与证据 ID 不被替换。新/旧分支编号均有 + 独立授权，历史扫描不会被后来一次复核覆盖。训练不足、持续姿态歧义和参考变化不靠重复验证解决。 +- 上游准备阶段按物理 Tag 关系保存旁观角点、候选及原指令/反馈身份，沿用有界运动分箱。 + 末节保持基准指令和反馈稳定才可采用。候选使用固定时间划分、原像素门限、角度分层的独立 + 误差比较及 Holm 控制；不能用最低误差强选不可区分的候选。 +- 相对位姿估计传播同图父 Tag 的像素不确定度，并保守累加父模型几何不确定度；后续末节 + 准备仍通过原运动、图像、可观测性和候选区分门限。回读核对模型身份、原始角点、固定取样、 + 保持通道和测量结果。短暂内部恢复仅沿既有带日志的参考延续规则复用来源。 + 旁观证据与已有共享 Tag 桥接并存时，策略标识同时保留两者的来源和约束方式。 + +## 计算和冻结 + +训练工作进程在会话内复用。传输、计算和返回均受同一超时/取消约束。任务索引缓存已选 +训练快照，增补只使受影响任务的局部模型失效，随后重算全局空间解。 + +`staged_training_decision` 记录输入与计划哈希、结构化问题、实际独立轮数、置信区间和 +增补/冻结/失败决定。`frozen_training.json` 使用显式 JSON 数值类型，包含双向映射、空间解、 +实际应用零偏、统计、掌部姿态和 Tag 安装；绑定完整配置、源 URDF、训练记录、零位和求解版本。 +修改求解语义时必须升级求解版本，不能用缓存替代证据。 + +在线最终验收加载已冻结结果，不再次运行曲线和空间训练优化。产物顺序仍为：独立角度验证、 +写 JSON、从落盘 JSON 重建 URDF、文件/限位/方向/通道/单位/像素检查、成对发布。 +离线审计复算训练决定并比对冻结结果。模型或证据改变时拒绝发布。五个末节绝对零位仍为 +明确的 CAD 假设，精度声明限于经过独立验证的运动范围和姿态。 + +## 断点和恢复 + +新旧计划不得混用。旧策略保持原恢复语义。新策略仅复用连续完整任务访问；部分方向不算 +访问完成。固定参考通过后，先导入原模型和原零位，再逐次复核当前零位，最后才跳过原访问。 +集中补采与冻结决定按日志顺序恢复；已冻结而缺失原训练前缀或冻结文件时拒绝回退训练。 +恢复父模型时一并恢复其已绑定的有界旁观原始帧，缺失角点不补造；当前零位仍须重新复核。 +扫描时间必须晚于所绑定访问的零位复核；导入历史扫描保留当时的复核证据，不能伪造新采集时间。 +准备缺样额度与同速重扫额度跨断点保留；已经耗尽的恢复不能通过重新加载断点获得新额度。 + +## 验证与局限 + +### 共享 Tag 改变进入姿态时的联合准备（2026-09-20） + +拇指 CMC roll 与 MCP 使用同一个 Tag，但两任务间 yaw 基准不同，不能直接把前一个零位 +当作 MCP 零位。本次保留既有 yaw 进入动作，把其原始图像与 MCP 自身准备图像联合求解： +前端绑定已经实测的 roll 零位，后端用保持指令及稳定反馈确认与 MCP 的共同姿态。 +SDK 指令只标识动作和保持状态，不充当相机测得的角度。没有新增运动或修改 Tag、速度、路径。 + +采样分箱及训练/留出划分事先固定，完整枚举两段动作的候选组合；训练决定模型，留出只可 +否决。维持原像素、几何不确定度和候选区分门限。目标几何的不确定度计算恢复全部自由度， +避免把固定来源零位误当作额外精度。辅助进入数据缺失时，在拟合前记录原因并使用原准备入口； +进入数据违反参考/安装/保持通道约束，或联合拟合及留出失败时，不能退回另一种求解碰运气。 + +`preparation_transition_v1` 绑定来源零位、来源模型、实际运动依赖、完整原始帧哈希和固定 +选样身份。日志回读重新核对源 URDF 依赖、来源零位图像，并复算联合模型、核对冻结模型哈希。 +取消和跨会话会清除未完成的进入证据。该机制沿用现有准备工作进程及状态机，普通冻结图像模型 +和最终 JSON→URDF 发布契约不变。 + +真实会话 `20260920_125306` 的 MCP 原始图像经运行时采集入口回放后通过：原方法仍因候选 +不可区分而拒绝；联合模型的最大训练/留出帧 RMS 分别约 1.117/1.083 px（原门限 1.5 px）。 +四个初始化组合中两个收敛到等价几何，另两个违反原像素门限。原中指 DIP 不充分证据回归仍拒绝。 +这证明该次 MCP 停点的软件问题得到改善，不代表整手物理精度、总耗时或一次连续通过已经验收。 +本次优化未启动硬件，也未生成或发布新的整手 JSON/URDF；默认策略继续为 `fixed`。 +软件验证与原始回放报告保存在 +`calibration_output/O30_RIGHT_001/software_review_preparation_transition/`。 + +测试按快速单元、回放、产物集成、历史兼容分层；命令见 `TESTING.md`。整手输入和产物由 +共享夹具提供，篡改、缺字段、取消等直接进入对应拒绝边界。修正了一个拿不完整采集数据 +调用最终化、却期待后置运动证据错误的旧用例，改为直接检验其负责的运动验收边界。 + +已覆盖：两轮冻结、一次局部/全手补采、补采失败、真实依赖的混合轮数、冻结后不再训练、 +留出隔离、回访复核、有界恢复、完整访问恢复、模型篡改拒绝、取消发布和 O30 JSON→URDF 一致性。 +真实中指 DIP 旧数据缺少旁观证据时仍拒绝。新增合成独立证据能区分候选时才通过。 +各次测试的源码指纹、耗时、失败及修复后复查关系保存于工作区 +`calibration_output/O30_RIGHT_001/software_review_staged_training/software_validation.json`。 + +O30 产物集成使用原正式采集证据入口及独立 FK 合成图像，复用兼容的几何运动授权。 +它覆盖文件链路与证据契约；在线图像准备算法另由原真实 DIP 回放和新增旁观图像用例检查。 +该合成产物用例不是整手实机图像模型选择、连续运动或物理精度证明。 + +实机工作仍需在全新会话完成：所有尝试均计入，比较总耗时、完整通过、人工干预和失败原因。 +记录开始到终态、准备、稳定等待、采样、恢复、训练、文件验收及开始前断点加载开销；比较时 +不能漏掉加载开销或只比较扫描时间。达到正确性门限、人工干预未增加且总耗时改善至少 10% +才考虑切换默认。大日志存储/全量解码重构未与本次采集规则一起进行。 diff --git a/src/linkerhand_calibration/TESTING.md b/src/linkerhand_calibration/TESTING.md index bfd2aa4..02c97c7 100644 --- a/src/linkerhand_calibration/TESTING.md +++ b/src/linkerhand_calibration/TESTING.md @@ -21,7 +21,7 @@ export PYTHONPATH="$PWD/src/linkerhand_calibration:${PYTHONPATH:-}" export PYTHONDONTWRITEBYTECODE=1 ``` -采集计划、训练接口日常验证： +采集计划、训练接口日常验证（按改动选择文件，不必每次全部执行）： ```bash python3 -m pytest -q -m quick \ @@ -35,6 +35,67 @@ python3 -m pytest -q -m quick \ `test_passed_resume.py`；涉及发布时加 `test_artifact_publication.py`。真实中指 DIP 歧义回归位于 `test_measured_transfer.py::test_real_middle_dip_fits_but_remains_ambiguous`，其正确结果仍为拒绝不充分证据。 +`test_prepared_resume.py::test_resume_tick_uses_prepared_data_and_still_checks_current_reference` +同时覆盖 O30 分阶段恢复：历史证据索引须在启动准备时完成，实时回调不能再次整理整份历史； +参考变化仍拒绝复用。五个型号、参考保持/变化共 10 项约 2 秒。真实 `20260920_141822` +在该索引阶段发生控制回调阻塞，修复后的 `20260920_142046` 已实际复用 4 个扫描单元； +后续因小指零位 ID4 检测断续停止，不代表整手通过。 + +O30 回访反馈重复性修改，先运行 `test_staged_training.py -k revisit`：真实反馈 48→41、 +10 的边界、超限、同批不稳定、图像/安装变化以及 v1/v2 回读共 8 项约 1.2 秒。 +涉及复核记录格式时，追加 `test_staged_artifacts.py` 的 JSON→URDF 一致性和阶段拒绝组， +共享一次整手产物，5 项约 31 秒；无需为每个拒绝条件重复完整拟合。 + +分阶段调度、旁观证据和恢复的快速检查： + +```bash +python3 -m pytest -q -m quick \ + src/linkerhand_calibration/test/test_staged_training.py \ + src/linkerhand_calibration/test/test_staged_runtime.py \ + src/linkerhand_calibration/test/test_observer_pose.py \ + src/linkerhand_calibration/test/test_observer_reference.py \ + --durations=8 --timings-json=/tmp/calibration-staged-quick.json +``` + +`test_observer_reference.py` 共享一次上下游图像求解，角点、来源、安装和取样篡改直接检验 +证据回读边界。`test_staged_runtime.py` 的进程复用用例及整手训练用例标为 `integration`。 +只有训练/冻结/发布契约变化时，才运行下面的分阶段整手组；本次初次完整组 19 项约 45 秒， +后增加的回访时序拒绝用例随产物组单独复查，未重复其余已通过的完整拟合： + +```bash +python3 -m pytest -q \ + src/linkerhand_calibration/test/test_staged_training.py \ + src/linkerhand_calibration/test/test_staged_runtime.py \ + src/linkerhand_calibration/test/test_staged_artifacts.py \ + --durations=8 --timings-json=/tmp/calibration-staged-integration.json +``` + +该组共享整手输入和产物，包含实际混合轮数统计、冻结后不再训练、一次集中补采、失败终止、 +断点及 JSON→URDF 链路。下面的旧策略整手组仅在旧策略相关逻辑也受影响时追加。 + +共享 Tag 的进入姿态联合准备修改，运行真实 MCP 的专门回放组： + +```bash +python3 -m pytest -q \ + src/linkerhand_calibration/test/test_preparation_transition.py \ + --durations=8 --timings-json=/tmp/calibration-preparation-transition.json +``` + +该组共享一次真实图像求解，检查正式运行时继续、原始证据回读、缺失辅助数据、留出隔离、 +保持通道/参考变化、篡改拒绝及取消清理，通常约 10 秒。只有候选选择公共逻辑受影响时追加 +`test_image_model_selection.py` 的相关用例；已有通过的完整拟合不为文档或断言修正重复执行。 +本次验证记录位于 `calibration_output/O30_RIGHT_001/software_review_preparation_transition/`， +保留失败与针对性复查关系，不把软件回放写成实机通过。 + +测量几何候选的精度资格与图像评估分离，回归位于 `test_measured_transfer.py` 的 +`test_real_ip_compares_uncertain_alternative_before_authorizing`、 +`test_uncertain_alternative_cannot_be_frozen` 和 +`test_uncertain_alternative_still_vetoes_when_images_do_not_distinguish`。 +三项共享 `20260920_140424` 的 129 帧真实 IP 准备及父几何证据；精度不足的候选仍参与 +留出区分，不能冻结；无法区分仍拒绝。相关旧中指 DIP、候选选择及回读边界共 17 项 +耗时 5.39 秒，后补否决边界 1 项耗时 2.16 秒。记录位于 +`calibration_output/O30_RIGHT_001/software_review_ip_alternative/`，不代表整手实机通过。 + 完整 O30 产物链路按需单独执行： ```bash @@ -54,8 +115,83 @@ python3 -m pytest -q \ 同一版本通过的测试不无理由复跑；修复失败后只重跑失败及受新改动影响的用例。裸 `pytest` 仍会运行全量，标记不会悄悄关闭测试。分层收集可用 `--collect-only -m replay` 等命令检查。 -本次软件验证记录保存在工作区 +上一版逐任务自适应方案的软件验证记录保存在工作区 `calibration_output/O30_RIGHT_001/software_review_adaptive_training/software_validation.json`， 同目录保留各组耗时记录和历史数据对照。O30 正式产物链路与混合轮数统计约 44 秒， 恢复、断点及真实 DIP 歧义回归约 19 秒，在线训练评估与取消约 6 秒。记录保留开发期间 的失败及修复后复查关系，不将不同代码版本的增量检查称为一次最终全量回归。 + +本次分阶段方案记录在 +`calibration_output/O30_RIGHT_001/software_review_staged_training/software_validation.json`。 +同目录保留各次运行的源码指纹、耗时及开发失败，报告标明修复关系。新旧策略、旁观图像、 +原真实 DIP 拒绝和文件链路均分别记录；不重复旧的 2.88 GB 日志性能测量，不宣称实机已通过。 + +遮挡感知准备策略的日常检查使用 `test_preparation_witness.py`：真实无名指停点保留为 +`fixtures/o30_ring_mcp_ambiguous.json.gz`，合成独立侧摆源明确标注，模块共享一次原始 +拟合和一次新增证据。覆盖逐指避让后单通道往返、整数 SDK 目标、回访不重采、 +训练/留出隔离、姿态改变、完整窗口、篡改、强制证据缺失及正式读回。只读策略为 +`--preparation-policy occlusion_aware_witness_v1 --validate-only`，不启动硬件。 + +最终遮挡版：该文件加 Profile 加载、采集计划、原图像采集契约，共 58 项约 14.12 秒。 +源码迭代与现场遮挡澄清导致的增量复查分别记录，不把开发过程累计次数称为一次回归。 +记录目录为 `calibration_output/O30_RIGHT_001/software_review_preparation_witness/`。 +新策略仍需完整实机对照；原 O30 JSON→URDF 及取消发布两项共享整手夹具约 32.12 秒， +没有逐个拒绝条件重新拟合整手，也没有运行全量 pytest/colcon。 + +准备优化器统一恢复检查位于 `test_image_bundle_optimizer.py`。快速用例检查计算次数、 +内存上限、不收敛/异常/非有限值/误差增大时继续拒绝；真实回放标记为 `replay`，共享 +一次 `20260920_154722` 拇指 IP 求解。夹具保留真实父参考、上游准备记录及其原始角点， +通过正式准备、来源几何和父参考验收；不能仅用孤立合成输出代替这些入口。 + +该次真实候选原先耗尽 150 次稀疏求值，使用同一训练证据暖启动直接解法后，再 6 次 +收敛，随后被原 1.5 px 门限拒绝，合格候选得以通过。未收敛候选仍否决，不参与绕过 +门限的比较；留出数据不决定计算恢复。主回归 24 项约 32.20 秒，覆盖旧 IP 精度不足 +候选、真实中指/无名指歧义拒绝、转接/共享姿态回读、小指历史预算、训练/留出隔离和 +工作进程取消。新增用例开发失败及针对性复查分别保存于 +`calibration_output/O30_RIGHT_001/software_review_solver_recovery/`。 +未重跑整手拟合和 JSON→URDF 产物组；本次改变的是准备求解及其证据绑定，产物生成、 +最终角度/像素门限没有修改。软件回放通过不代表整手实机已经通过。 + +准备采样有效性与辅助适用性使用 `test_preparation_sync.py`：夹具来自真实 +`20260920_160630` 小指完整准备窗口，保留两帧原始缺指令值。检查完整原始窗口回读、 +排除记录绑定、缺值行上的保持关节变化、持续缺样/端点/身份变化拒绝，以及拟合前 +辅助不适用时主模型独立验收，拟合后不改路线。与 `test_preparation_witness.py` 的 +原歧义和篡改检查合计 41 项、10.58 秒；历史 v1 和拟合失败拒绝另 3 项、3.97 秒。 +后补缺候选不可视为转角不足的边界单独执行；没有无理由重跑前述已通过组。 +验证记录位于 `calibration_output/O30_RIGHT_001/software_review_preparation_admission/`。 + +双端共享姿态交接使用 `test_shared_pose_handoff.py`，夹具来自 `20260920_163002`， +包括原 MCP 模型/零位/像素来源、上游回基准原始窗口和 PIP 准备完整证据。 +共享一次 PIP 拟合，检查原失败复现、新双端验证、原门限、正式回读、数据篡改、验证轮 +隔离、源图像先于全部新准备数据、断点依赖导入/去重、恢复代次和四指模型归属。 +DIP 用真实像素模拟后续访问，经 `ObservationCapture`、父参考确认和文件回读， +下游模型外壳明确为合成数据，不能宣称整指实机通过。 + +本轮父参考、有限恢复、旧断点及旧版共享姿态 62 项通过，20.95 秒；最终针对组 +25 项通过，4.11 秒（含 1 项已受修改影响的旧父参考回读）。随后新增时序边界与导入 +去重只执行相应用例。现有 O30 正式采集证据入口 JSON→URDF 集成单独运行一次， +31.24 秒。开发中的夹具缺来源、策略字段绑定和 ROS 环境收集失败保留原记录， +不计为通过；没有全量 pytest 或 colcon。完整记录见 +`calibration_output/O30_RIGHT_001/software_review_shared_pose/`。 + +准备观测预算的实机像素回归使用 `test_preparation_density.py`，夹具为 +`20260920_171710` 的完整小指 PIP/DIP 准备窗口、上游模型和双端交接图像。新 PIP +模型重新投影原 DIP 像素后，验证更完整的训练/留出分组、源模型精度传递、候选区分 +及模型回读；冻结旧父模型仍拒绝，四指多关节求解不扩容。此回放带有明确的离线姿态 +准入标记，不声称它是一次可发布的实机采集。正式产物另用已有 staged 产物组检验。 + +`test_checkpoint_startup.py` 检查断点加载心跳、独立 900 秒上限、恢复后设备/服务 +时限、异常清理及无断点不发布加载状态。相关启动/共享姿态/真实密集回放 98 项 +14.79 秒；产物回读、阶段隔离和取消发布 8 项 24.11 秒。前一批 85 项通过、1 项 +失败是历史测试将最大像素误差写死为 0.75；新增留出图像为 0.752，实际契约为 +1.5 px。现已断言正式契约，并复查完整共享姿态回读。环境收集失败、反事实回放 +拒绝分类断言修正和后续通过记录均保留，不混算为一次完整实机回归。 + +扩展到准备采集/辅助证据入口后，发现在线密集预算未传入离线重拟合，已补齐 +`image_sampling_policy` 的版本、报告与角色哈希绑定、重放参数及帧数上限校验。 +后续来源/辅助/真实回放组 121 项通过、2 项为测试断言失败；原断言分别固定了错误 +文本和旧采样子集模型。修正为准确拒绝码及相同预算的离线对照后，仅复查失败用例、 +受影响的启动与产物边界，16 项通过，32.54 秒。发布线程清理再检查 7 项通过。 +记录完整保存在 `software_review_stability_density`，没有将多版本增量测试宣称为 +一次全量回归。实机尝试 `20260920_174317` 在发起标定运动前被现有 ROS 控制节点占用 +阻止，未产生可交付 JSON/URDF。 diff --git a/src/linkerhand_calibration/config/o30_right_product.yaml b/src/linkerhand_calibration/config/o30_right_product.yaml index 3df1fe2..e1faf70 100644 --- a/src/linkerhand_calibration/config/o30_right_product.yaml +++ b/src/linkerhand_calibration/config/o30_right_product.yaml @@ -26,8 +26,8 @@ cameras: artifacts: source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b - camera_extrinsics: config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml - camera_extrinsics_sha256: 59f587e8e371f09456043c17cd0268fe1f170f1b7efdf9d2cb744c73c194953c + camera_extrinsics: config/o30_three_camera_extrinsics_20260920_123651.yaml + camera_extrinsics_sha256: e21d21eaaf198d4daba44a44e0aa3bc46a86376213b84c3ddd30ae571714c701 calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml calibration_config_sha256: 95a8fd3299151755ea3b5209b74b9ec9de1c2d07a05d557e9018e742fb66ce30 tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index f074a24..dfa4a69 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -301,6 +301,10 @@ def _launch_stack(context): ), "resume_mode": LaunchConfiguration("resume_mode"), "training_policy": LaunchConfiguration("training_policy"), + "preparation_witnesses_json": ParameterValue( + LaunchConfiguration("preparation_witnesses_json"), value_type=str), + "capture_plan_expected_sha256": ParameterValue( + LaunchConfiguration("capture_plan_expected_sha256"), value_type=str), "initial_command_file": LaunchConfiguration("initial_command_file"), "command_topic": command_topic, "state_topic": state_topic, @@ -461,6 +465,8 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("resume_raw_samples_path", default_value=""), DeclareLaunchArgument("resume_mode", default_value="verify"), DeclareLaunchArgument("training_policy", default_value="fixed"), + DeclareLaunchArgument("preparation_witnesses_json", default_value=""), + DeclareLaunchArgument("capture_plan_expected_sha256", default_value=""), DeclareLaunchArgument("initial_command_file", default_value=""), DeclareLaunchArgument( "calibration_config", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py index 33df39c..407d0cf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py @@ -12,6 +12,7 @@ import json PLAN_VERSION = "capture_plan_v1" FIXED = "fixed" ADAPTIVE = "adaptive_2_to_3" +STAGED = "staged_2_to_3" LEGACY_TRAINING = (0, 1, 2) LEGACY_HOLDOUT = 3 @@ -53,6 +54,8 @@ class CapturePlan: motion = {"unit": profile.command.unit, "motion": asdict(profile.motion), "nodes": {task.key: {"training": command_nodes(profile, task), "holdout": command_nodes(profile, task, holdout=True)} for task in profile.motion.tasks}} + if not profile.motion.preparation_witnesses: + motion["motion"].pop("preparation_witnesses") # Historical plan hashes stay unchanged. return cls(profile.quality.training_policy, profile.acquisition.command_capture_mode, tasks, json.dumps(motion, sort_keys=True, separators=(",", ":"), allow_nan=False)) @@ -60,13 +63,20 @@ class CapturePlan: return next(task for task in self.tasks if task.task_key == key) def as_dict(self): - return {"version": PLAN_VERSION, "policy": self.policy, + result = {"version": "capture_plan_v2" if self.policy == STAGED else PLAN_VERSION, "policy": self.policy, "command_capture_mode": self.command_capture_mode, "motion_contract": json.loads(self.motion_contract_json), "tasks": {t.task_key: {"training_cycles": list(t.training), "holdout_cycle": t.holdout, "command_training_cycles": list(t.command_training), "command_holdout_cycle": t.command_holdout} for t in self.tasks}} + if self.policy == STAGED: + result["stages"] = [ + {"name": name, "cycle": cycle, "tasks": [t.task_key for t in self.tasks + if cycle in t.motion_cycles]} + for name, cycle in (("first_training", 0), ("second_training", 1), + ("supplement", 2), ("validation", 3))] + return result @property def sha256(self): @@ -87,7 +97,8 @@ def task_partition(profile, task=None, *, joint=None): if joint is not None: task = task_for_joint(profile, joint) key = getattr(task, "key", task) - training = tuple(profile.quality.task_training_cycles.get(key, profile.quality.training_cycles)) + default = LEGACY_TRAINING[:2] if profile.quality.training_policy == STAGED else profile.quality.training_cycles + training = tuple(profile.quality.task_training_cycles.get(key, default)) holdout = profile.quality.holdout_cycle separate = profile.acquisition.command_capture_mode == "separate" return TaskPartition(key, training, holdout, (holdout + 1,) if separate else training, @@ -104,20 +115,20 @@ def select_task_training(profile, task_key, cycles): raise ValueError("capture_plan_unknown_task") if cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]): raise ValueError("capture_plan_invalid_training_partition") - if cycles != tuple(profile.quality.training_cycles) and profile.quality.training_policy != ADAPTIVE: + if cycles != tuple(profile.quality.training_cycles) and profile.quality.training_policy not in {ADAPTIVE, STAGED}: raise ValueError("capture_plan_fixed_training_cannot_change") selections = {**profile.quality.task_training_cycles, task_key: cycles} return replace(profile, quality=replace(profile.quality, task_training_cycles=selections)) def configure_training(profile, policy): - if policy not in {FIXED, ADAPTIVE}: + if policy not in {FIXED, ADAPTIVE, STAGED}: raise ValueError("unknown_training_policy") if profile.artifacts.output_schema_version in {2, 3} and ( tuple(profile.quality.training_cycles) != LEGACY_TRAINING or profile.quality.holdout_cycle != LEGACY_HOLDOUT): raise ValueError("production_partition_requires_versioned_training_policy") - if policy == ADAPTIVE and (profile.artifacts.output_schema_version < 3 + if policy in {ADAPTIVE, STAGED} and (profile.artifacts.output_schema_version < 3 or not profile.command_based_release or profile.acquisition.command_capture_mode != "interleaved" or tuple(profile.quality.training_cycles) != LEGACY_TRAINING diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index d9b374e..c08aefa 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -269,6 +269,9 @@ class MotionPolicy: return_groups: tuple[tuple[int, ...], ...] = () resume_verification_waypoints: tuple[ReferenceWaypoint, ...] = () joint_zero_references: Mapping[str, JointZeroSpec] = field(default_factory=dict) + # Target joint -> bounded upstream excitation at its clearance posture. + # These images only authorize preparation; they never enter formal fitting. + preparation_witnesses: Mapping[str, ModelPreparation] = field(default_factory=dict) @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py index 5e2a538..9c07067 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py @@ -94,12 +94,12 @@ def fit_command_training(profile, source_model, sweep, steady, references): return CommandMotionFit(curves, curves, mappings, metrics, applicability, inputs, {}) -def fit_command_motion(profile, source_model, sweep, steady, references): +def fit_command_motion(profile, source_model, sweep, steady, references, *, frozen_motion=None): train_sweep = {name: [r for r in rows if r['cycle'] in training_cycles(profile, joint=name)] for name, rows in sweep.items()} train_steady = {name: [r for r in rows if r['cycle'] in command_training_cycles(profile, joint=name)] for name, rows in steady.items()} - motion = fit_command_training(profile, source_model, train_sweep, train_steady, references) + motion = frozen_motion or fit_command_training(profile, source_model, train_sweep, train_steady, references) held = {name: [r for r in rows if r['cycle'] == command_holdout_cycle(profile)] for name, rows in steady.items()} metrics = validate_command_training(profile, motion.coordinate_curves, diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/frozen.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/frozen.py new file mode 100644 index 0000000..cf548dc --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/frozen.py @@ -0,0 +1,60 @@ +"""Explicit JSON transport for immutable numerical training objects. + +Only a fixed registry of calibration value types can be reconstructed. There +is no pickle, dynamic import or executable object in a persisted model. +""" + +from dataclasses import fields, is_dataclass +import math + +import numpy as np +from scipy.spatial.transform import Rotation + + +def encode(value): + if isinstance(value, Rotation): + return {"$type": "Rotation", "value": value.as_quat().tolist()} + if isinstance(value, np.ndarray): + return {"$type": "array", "value": value.tolist()} + if isinstance(value, np.generic): + return encode(value.item()) + if is_dataclass(value): + return {"$type": type(value).__name__, "fields": { + field.name: encode(getattr(value, field.name)) for field in fields(value)}} + if isinstance(value, dict): + return {"$type": "mapping", "items": [[encode(k), encode(value[k])] for k in sorted(value, key=repr)]} + if isinstance(value, (tuple, list, frozenset, set)): + values = sorted(value) if isinstance(value, (set, frozenset)) else value + return {"$type": type(value).__name__, "items": [encode(item) for item in values]} + if isinstance(value, float) and not math.isfinite(value): + raise ValueError("frozen_training_nonfinite_value") + if value is None or isinstance(value, (str, int, float, bool)): + return value + raise TypeError(f"unsupported_frozen_training_type:{type(value).__name__}") + + +def decode(value): + if not isinstance(value, dict): + return value + kind = value.get("$type") + if kind == "Rotation": + return Rotation.from_quat(value["value"]) + if kind == "array": + return np.asarray(value["value"], dtype=float) + if kind == "mapping": + return {decode(k): decode(v) for k, v in value["items"]} + sequences = {"tuple": tuple, "list": list, "set": set, "frozenset": frozenset} + if kind in sequences: + return sequences[kind](decode(item) for item in value["items"]) + from . import command_motion, tag_installation + from .spatial_solver import stages, solve, types + from ..domain import measurement, result + registry = {name: cls for module in (command_motion, tag_installation, stages, solve, + types, measurement, result) for name, cls in vars(module).items() + if isinstance(cls, type) and is_dataclass(cls)} + if kind not in registry: + raise ValueError(f"unknown_frozen_training_type:{kind}") + cls = registry[kind] + if set(value["fields"]) != {field.name for field in fields(cls)}: + raise ValueError("frozen_training_fields_changed") + return cls(**{key: decode(item) for key, item in value["fields"].items()}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py index 7de45f1..5d13ad3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py @@ -9,7 +9,7 @@ from typing import Mapping from ..domain.profile import CalibrationProfile, validate_profile from ..domain.result import CalibrationResult, JointMapping -from ..domain.capture_plan import CapturePlan, ADAPTIVE, training_cycles +from ..domain.capture_plan import CapturePlan, ADAPTIVE, STAGED, training_cycles from ..urdf.kinematics import UrdfKinematicModel from .motion_fit import channel_for_joint, fit_profile_motion, joint_input_direction from .spatial import ZeroCalibrationProfile, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection @@ -98,7 +98,7 @@ def build_axis_observations(profile, model, geometry_records, motion, zero_profi rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows] parent = zero_profile.phase_parent_joint.get(joint) if parent is not None and parent not in by_joint: - if cycle != plan.task(task.key).holdout and profile.quality.training_policy == ADAPTIVE: + if cycle != plan.task(task.key).holdout and profile.quality.training_policy in {ADAPTIVE, STAGED}: continue # No same-cycle parent observation: do not fabricate a geometric sample. raise ValueError("spatial axis order must place phase parents first") task = next(task for task in profile.motion.tasks if joint in task.joints) @@ -120,7 +120,8 @@ def build_axis_observations(profile, model, geometry_records, motion, zero_profi def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint, - *, cross_view_records=None, zero_references=None, steady_records=None, motion_fitted=lambda _motion: None) -> CalibrationResult: + *, cross_view_records=None, zero_references=None, steady_records=None, motion_fitted=lambda _motion: None, + frozen_training=None) -> CalibrationResult: """Shared native-domain motion, spatial zero and standard mimic fit. No mechanical endpoint or range-centre fallback can replace a requested @@ -150,7 +151,8 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco zero_profile = compile_spatial_profile(profile) if profile.command_based_release: from .command_motion import fit_command_motion - motion = fit_command_motion(profile, model, records_by_joint, steady_records or {}, zero_references or {}) + motion = fit_command_motion(profile, model, records_by_joint, steady_records or {}, zero_references or {}, + frozen_motion=None if frozen_training is None else frozen_training["motion"]) geometry_records = steady_records geometry_domain = f"command_{profile.command.unit}" else: @@ -180,7 +182,8 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations, curves=spatial_curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves}, zero_profile=zero_profile, training_cycles=geometry_training, validation_cycle=geometry_holdout, - allow_partial_training_cycles=profile.quality.training_policy == ADAPTIVE, + allow_partial_training_cycles=profile.quality.training_policy in {ADAPTIVE, STAGED}, + frozen_training=None if frozen_training is None else frozen_training["zero"], maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20), maximum_validation_mae_rad=math.radians(1), maximum_validation_p95_rad=math.radians(2), maximum_validation_error_rad=math.radians(3), maximum_confidence_half_width_rad=math.radians(1), diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py index 19afc63..58e8151 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py @@ -88,6 +88,15 @@ def decide_acceptance( failure_reasons=failures, passed=passed) +def applied_output_offsets(profile, applied_training, output_offsets, product_finger_rolls, common_mode): + """The same training-frozen coordinate convention for Tag fit and release.""" + offsets = {**applied_training, **output_offsets} + if profile.parallel_root_pattern: + for name in product_finger_rolls: + offsets[name] = float(offsets[name]) - common_mode + return offsets + + def assemble_result( validation_cycle: int, *, @@ -131,24 +140,8 @@ def assemble_result( # 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) - # 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.parallel_root_pattern 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 - ) - + final_offsets = applied_output_offsets(profile, applied_training, output_offsets, + product_finger_rolls, 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 # reference finger's assembly offset. Preserve the source-CAD zero for diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py index da0d94b..4672d0f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py @@ -12,7 +12,7 @@ from .acceptance import assemble_result, decide_acceptance, decide_training_acce from .optimization import solve_selected from .preparation import prepare_offset_limits, prepare_zero_problem from .residuals import ObservationGeometry -from .stages import TrainingFit, TrainingProblem, ZeroSolveOptions +from .stages import TrainingFit, TrainingProblem, ZeroSolveOptions, ObservabilityEvidence, CycleEvidence from .statistics import estimate_cycle_evidence, estimate_observability from .types import ( JointAxisMeasurement, @@ -23,12 +23,23 @@ from .types import ( from .validation import check_observation_geometry, validate_axis_lines, validate_holdout +@dataclass(frozen=True) +class FrozenZeroTraining: + input_sha256: str + fit: TrainingFit + observability: ObservabilityEvidence + cycles: CycleEvidence + applied_offsets: Mapping[str, float] + + @dataclass(frozen=True) class ZeroTrainingResult: offsets: Mapping[str, float] confidence_half_widths: Mapping[str, float] cycle_values: Mapping[str, Sequence[float]] failures: tuple[str, ...] + frozen: FrozenZeroTraining | None = None + failure_reasons: Mapping[str, str] | None = None def fit_urdf_zero_training(*, measurements, training_cycles, **kwargs): @@ -71,6 +82,7 @@ def solve_urdf_zero_offsets( static_output_zero_offsets_rad: Mapping[str, float] | None = None, zero_profile: ZeroCalibrationProfile | None = None, allow_partial_training_cycles: bool = False, + frozen_training: FrozenZeroTraining | None = None, ) -> ZeroSolveResult | ZeroTrainingResult: """Prepare, fit training only, validate the frozen result, then assemble.""" options = ZeroSolveOptions( @@ -107,24 +119,45 @@ def solve_urdf_zero_offsets( training_geometry = geometry.for_cycles(training_cycles) training_problem = TrainingProblem(problem.profile, problem.training, problem.fixed_offsets, problem.zero_offsets) limits = prepare_offset_limits(problem=problem, options=options) - rotation, translation, offsets = solve_selected( - problem=training_problem, limits=limits, geometry=training_geometry, selected=problem.training) - fit = TrainingFit(rotation, translation, offsets) - observability = estimate_observability(problem=training_problem, fit=fit, geometry=training_geometry) + from ..frozen import encode + from ...domain.capture_plan import evidence_digest + import hashlib + input_sha256 = evidence_digest(encode((tuple(problem.training), tuple(training_cycles), + curves, motor_by_joint, options, problem.profile, + hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest(), + training_geometry.orientation_by_model_cycle, allow_partial_training_cycles))) if ( + validation_cycle is None or frozen_training is not None) else "" + if frozen_training is not None: + if frozen_training.input_sha256 != input_sha256: + raise ValueError("frozen_zero_training_inputs_changed") + fit, observability, cycles = (frozen_training.fit, frozen_training.observability, frozen_training.cycles) + rotation, offsets = fit.base_rotation, fit.training_offsets + else: + rotation, translation, offsets = solve_selected( + problem=training_problem, limits=limits, geometry=training_geometry, selected=problem.training) + fit = TrainingFit(rotation, translation, offsets) + observability = estimate_observability(problem=training_problem, fit=fit, geometry=training_geometry) + cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options, + geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles) geometry_evidence = check_observation_geometry(problem=problem, options=options, geometry=geometry, selected=list(measurements), offsets=offsets, rotation=rotation) - cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options, - geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles) if validation_cycle is None: decision = decide_training_acceptance(problem=problem, limits=limits, fit=fit, options=options, observability=observability, geometry_evidence=geometry_evidence, cycles=cycles) + from .acceptance import applied_output_offsets + applied = applied_output_offsets(problem.profile, cycles.applied_training, problem.output_offsets, + limits.product_finger_rolls, decision.finger_roll_common_mode) return ZeroTrainingResult(offsets, cycles.confidence_half_widths, cycles.cycle_values, - tuple(f"{reason}:{name}" for name, reason in sorted(decision.failure_reasons.items()))) + tuple(f"{reason}:{name}" for name, reason in sorted(decision.failure_reasons.items())), + FrozenZeroTraining(input_sha256, fit, observability, cycles, applied), dict(decision.failure_reasons)) holdout = validate_holdout(problem=problem, fit=fit, cycles=cycles, options=options, geometry=geometry, measurements=measurements) lines = validate_axis_lines(problem=problem, fit=fit, cycles=cycles, geometry=geometry) decision = decide_acceptance(problem=problem, limits=limits, fit=fit, options=options, observability=observability, geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout) - return assemble_result(problem=problem, limits=limits, fit=fit, observability=observability, + result = assemble_result(problem=problem, limits=limits, fit=fit, observability=observability, geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout, lines=lines, decision=decision, validation_cycle=validation_cycle) + if frozen_training is not None and dict(result.direct_offsets_rad) != dict(frozen_training.applied_offsets): + raise ValueError('frozen_zero_applied_offsets_changed') + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py index d1452b6..6918b33 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py @@ -22,9 +22,8 @@ def unconstrained_hinge_linearization(bundle, fit, angles): return free, linearization -def hinge_uncertainty(bundle, fit): - if hasattr(bundle, "uncertainty_problem"): - bundle, fit = bundle.uncertainty_problem(fit) +def image_fit_covariance(bundle, fit): + """Full joint covariance, including every nuisance image angle.""" jacobian = fit.jac.toarray() if hasattr(fit.jac, "toarray") else np.asarray(fit.jac) scaled = jacobian * bundle.scale[None, :] _, singular, vh = np.linalg.svd(scaled, full_matrices=False) @@ -32,6 +31,46 @@ def hinge_uncertainty(bundle, fit): raise ValueError("image_motion_geometry_rank_deficient") variance = max(.03 ** 2, float(np.sum(fit.fun ** 2) / max(1, len(fit.fun) - len(fit.x)))) covariance = ((vh.T / singular ** 2) @ vh * variance) * bundle.scale[:, None] * bundle.scale[None, :] + return covariance + + +def preparation_pose_uncertainty(bundle, fit): + """Conservative pose bounds for an independent, single-hinge witness. + + Keep the full geometry/angle covariance. Repeated endpoint images do not + earn a sqrt(N) reduction; report the worst bound over training images. + """ + from scipy.spatial.transform import Rotation + from .motion_image_solver import ImageHingeBundle + if type(bundle) is not ImageHingeBundle or bundle.joints != 1: + raise ValueError("preparation_witness_uncertainty_graph_unsupported") + covariance = image_fit_covariance(bundle, fit) + angles = np.r_[0., fit.x[10:]] + def poses(parameters): + axis, reference, pivot, mount = bundle.geometry_values(parameters, 0) + rotations = Rotation.from_rotvec(np.r_[0., parameters[10:]][:, None]*axis)*reference + return rotations, pivot+rotations.apply(mount) + rotations, _ = poses(fit.x) + derivative = np.zeros((len(angles), 6, len(fit.x))) + for column in range(len(fit.x)): + step = 1e-5*bundle.scale[column] + upper, lower = fit.x.copy(), fit.x.copy() + upper[column] += step + lower[column] -= step + ru, tu = poses(upper) + rl, tl = poses(lower) + derivative[:, :3, column] = ((ru*rotations.inv()).as_rotvec() + -(rl*rotations.inv()).as_rotvec())/(2*step) + derivative[:, 3:, column] = (tu-tl)/(2*step) + propagated = derivative @ covariance @ np.swapaxes(derivative, 1, 2) + return tuple(float(3*np.sqrt(max(0., np.max(np.linalg.eigvalsh( + propagated[:, i:i+3, i:i+3]))))) for i in (0, 3)) + + +def hinge_uncertainty(bundle, fit): + if hasattr(bundle, "uncertainty_problem"): + bundle, fit = bundle.uncertainty_problem(fit) + covariance = image_fit_covariance(bundle, fit) axes, pivots, child_bounds = [], [], [] def coordinates(parameters, index): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_bundle_optimizer.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_bundle_optimizer.py new file mode 100644 index 0000000..066bb20 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_bundle_optimizer.py @@ -0,0 +1,70 @@ +"""Bounded training-only recovery for articulated image bundle fits. + +Sparse LSMR steps are fast, but may stagnate near an active bound. Only a +finite, budget-exhausted fit gets one warm start using the exact dense trust +region solver. Neither holdout images nor another candidate select that retry. +""" + +from dataclasses import dataclass + +import numpy as np +from scipy.optimize import least_squares + +IMAGE_BUNDLE_SOLVER_POLICY = "sparse_then_bounded_exact_v1" +# Covers all six relations / 72 training frames of the production defaults, +# including the simultaneous four-finger yaw task, without unbounded storage. +MAXIMUM_DENSE_PARAMETERS = 512 +MAXIMUM_DENSE_JACOBIAN_ENTRIES = 2_000_000 + + +@dataclass(frozen=True) +class ImageSolverAttempt: + backend: str + budget: int + evaluations: int + status: int + cost: float | None + optimality: float | None + + +def _attempt(backend, budget, fit): + def finite(value): + return float(value) if np.isfinite(value) else None + return ImageSolverAttempt(backend, budget, int(fit.nfev), int(fit.status), + finite(fit.cost), finite(fit.optimality)) + + +def _finite_fit(fit): + return (np.all(np.isfinite(fit.x)) and np.all(np.isfinite(fit.fun)) + and np.isfinite(fit.cost) and np.isfinite(fit.optimality)) + + +def solve_image_bundle(residual, initial, *, sparsity, scale, limits, bounds=(-np.inf, np.inf)): + """At most two solves, with unchanged residuals, bounds and tolerances. + +Already-converged historical fits retain the original numerical path. Large +graphs remain bounded in memory and fail closed if the sparse solve fails. +Worker cancellation/timeout still owns the outer wall-clock limit. +""" + options = dict(bounds=bounds, x_scale=scale, ftol=1e-9, xtol=1e-9, gtol=1e-7) + fit = least_squares(residual, initial, jac_sparsity=sparsity, + max_nfev=limits.maximum_solver_evaluations, + tr_options={"atol": 1e-10, "btol": 1e-10}, **options) + attempts = [_attempt("sparse_lsmr", limits.maximum_solver_evaluations, fit)] + # Offline diagnostic limits predate production recovery and retain their + # explicit single-solve budget. + budget = getattr(limits, "maximum_solver_recovery_evaluations", 0) + if (fit.status == 0 and budget > 0 and _finite_fit(fit) + and len(fit.x) <= MAXIMUM_DENSE_PARAMETERS + and len(fit.x)*len(fit.fun) <= MAXIMUM_DENSE_JACOBIAN_ENTRIES): + try: + recovered = least_squares(residual, fit.x.copy(), tr_solver="exact", + max_nfev=budget, **options) + attempts.append(_attempt("dense_exact", budget, recovered)) + # A failed or worse retry cannot manufacture convergence. + if _finite_fit(recovered) and recovered.cost <= fit.cost: + fit = recovered + except (ValueError, FloatingPointError, np.linalg.LinAlgError): + attempts.append(ImageSolverAttempt("dense_exact", budget, 0, -1, None, None)) + fit.image_solver_attempts = tuple(attempts) + return fit diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py index 65ea462..321d9c9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py @@ -12,6 +12,7 @@ from .motion_image_types import ImageHingeGeometry, ImageRoleQuality from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS from .image_reference import StationaryImageReference, image_reference_from_dict from .pose_bridge import PoseBridgeCheck +from .image_bundle_optimizer import ImageSolverAttempt IMAGE_MOTION_MODEL_POLICY = "image_hinge_v1_pre_zero_frozen" @@ -123,6 +124,9 @@ class ImageMotionHypothesis: child_frame_uncertainty: tuple[tuple[str, float, float], ...] = () source_geometry_checks: tuple[TransferGeometryCheck, ...] = () pose_bridge_checks: tuple[PoseBridgeCheck, ...] = () + transition_branches: tuple[tuple[str, int], ...] = () + preparation_pose_uncertainty: tuple[float, ...] = () + training_solver_attempts: tuple[ImageSolverAttempt, ...] = () @dataclass(frozen=True) @@ -132,6 +136,12 @@ class ImageMotionResolution: model: ImageMotionModel | None = None hypotheses: tuple[ImageMotionHypothesis, ...] = () selections: tuple[MotionRoleSelection, ...] = () + observer_pose_bridges: tuple[dict, ...] = () + observer_rejections: tuple[str, ...] = () + preparation_transitions: tuple[dict, ...] = () + selected_hypothesis: int | None = None + preparation_transition_rejections: tuple[str, ...] = () + preparation_witnesses: tuple[dict, ...] = () def image_motion_model_from_dict(raw: Mapping) -> ImageMotionModel: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py index 1781c87..06d4e79 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py @@ -35,11 +35,20 @@ class TransferGeometryCheck: def transfer_geometry_checks(geometry, constraints, sources, bounds): + """Require publishable precision, including during model readback.""" + checks, inherited, precision_accepted = assess_transfer_geometry(geometry, constraints, sources, bounds) + if not precision_accepted: + raise ValueError("image_motion_geometry_uncertain") + return checks, inherited + + +def assess_transfer_geometry(geometry, constraints, sources, bounds): """Match independent fixed-source hinges using their measured error bounds. - The same conservative lever-arm bound used by the former hard-constrained - solver is now used in candidate eligibility as well as reported uncertainty. - Geometry assembled jointly from current images retains its exact CAD solver. + Candidate evaluation is separate from authorization: insufficient precision + prevents freezing a model, but does not erase a converged image explanation + that must still challenge the winner on independent images. Invalid source + identities or topology remain errors, not evaluable competing models. """ if (not constraints or not sources or any(not isinstance(c, ParallelAxisGeometry) for c in constraints) or any(not isinstance(s, SourceHinge) for s in sources)): @@ -53,7 +62,7 @@ def transfer_geometry_checks(geometry, constraints, sources, bounds): or len(children) != len(constraints) or len(uncertainty) != len(bounds) or set(uncertainty) != set(children)): raise ValueError("measured_transfer_graph_unsupported") - checks, inherited = [], {} + checks, inherited, precision_accepted = [], {}, True limits = MotionEvidenceParameters() for c in constraints: source, child = parents[c.parent_joint], children[c.child_joint] @@ -66,7 +75,7 @@ def transfer_geometry_checks(geometry, constraints, sources, bounds): if (any(not math.isfinite(v) or v <= 0 for v in (axis_bound, point_bound)) or axis_bound+inherited_axis > limits.maximum_split_axis_difference_rad or point_bound+inherited_point > limits.maximum_frame_pivot_error_m): - raise ValueError("image_motion_geometry_uncertain") + precision_accepted = False axis = c.axis_sign*Rotation.from_quat(g.reference_quaternion_xyzw).inv().apply(g.axis_parent_xyz) measured_axis = np.asarray(child.axis_parent_xyz) angle = math.atan2(np.linalg.norm(np.cross(axis, measured_axis)), axis @ measured_axis) @@ -77,7 +86,7 @@ def transfer_geometry_checks(geometry, constraints, sources, bounds): checks.append(TransferGeometryCheck(c.child_joint, angle, inherited_axis, abs(float(distance)-c.distance_m), inherited_point)) inherited[c.child_joint] = (inherited_axis, inherited_point) - return tuple(checks), inherited + return tuple(checks), inherited, precision_accepted @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py index 77f675a..b6d90af 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py @@ -2,7 +2,6 @@ import math import numpy as np -from scipy.optimize import least_squares from scipy.sparse import lil_matrix from scipy.spatial.transform import Rotation @@ -10,6 +9,7 @@ from .motion_image_solver import ImageHingeBundle, _basis from .hinge_uncertainty import unconstrained_hinge_linearization from .measured_transfer import transfer_geometry_checks from .motion_image_types import ImageHingeGeometry +from .image_bundle_optimizer import solve_image_bundle class MeasuredTransferImageHingeBundle(ImageHingeBundle): @@ -72,9 +72,8 @@ class MeasuredTransferImageHingeBundle(ImageHingeBundle): sparsity[rows, self.geometry_parameter_indices(joint)] = 1 if frame: sparsity[rows, self.static_parameter_count+(frame-1)*self.joints+joint] = 1 - return least_squares(self.residual, self.initial, bounds=(self.lower, self.upper), - jac_sparsity=sparsity.tocsr(), x_scale=self.scale, max_nfev=limits.maximum_solver_evaluations, - tr_options={"atol": 1e-10, "btol": 1e-10}, ftol=1e-9, xtol=1e-9, gtol=1e-7) + return solve_image_bundle(self.residual, self.initial, bounds=(self.lower, self.upper), + sparsity=sparsity.tocsr(), scale=self.scale, limits=limits) def uncertainty_problem(self, fit): angles = np.vstack((np.zeros(self.joints), fit.x[self.static_parameter_count:].reshape(-1, self.joints))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py index f60a4e9..b1fa77b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py @@ -10,6 +10,7 @@ from .ippe import square_object_points from .motion_evidence import MotionEvidenceParameters, _axis, _fit_constraint from .relative import _relative_pose from .motion_image_types import ImageHingeGeometry +from .image_bundle_optimizer import solve_image_bundle def _basis(axis): @@ -142,14 +143,8 @@ class ImageHingeBundle: sparsity[rows, self.geometry_parameter_indices(ancestor)] = 1 if frame: sparsity[rows, self.static_parameter_count + (frame - 1) * self.joints + ancestor] = 1 - return least_squares(self.residual, self.initial, jac_sparsity=sparsity.tocsr(), - x_scale=self.scale, max_nfev=parameters.maximum_solver_evaluations, - # LSMR's default 1e-6 inner tolerance leaves coupled hinge/angle - # steps too approximate near the image minimum. Tighten that - # linear solve so the unchanged outer convergence tolerances can - # be reached within the bounded evaluation budget. - tr_options={"atol": 1e-10, "btol": 1e-10}, - ftol=1e-9, xtol=1e-9, gtol=1e-7) + return solve_image_bundle(self.residual, self.initial, sparsity=sparsity.tocsr(), + scale=self.scale, limits=parameters) def validate(self, parameters, frames, poses, limits): roots, matrices, observed, objects = self._image_inputs(frames, poses) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/observer_pose.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/observer_pose.py new file mode 100644 index 0000000..d1d8302 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/observer_pose.py @@ -0,0 +1,148 @@ +"""A held descendant's rigid pose from independent upstream camera angles. + +Every IPPE seed sees the same chronological train/holdout split. Alternatives +must be equivalent or separated by the existing independent image-loss test. +A held-out failure vetoes the training winner; it never selects another one. +""" + +from dataclasses import dataclass +import math + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation + +from .ippe import square_object_points +from .relative import _relative_pose + + +@dataclass(frozen=True) +class ObserverPose: + quaternion_xyzw: tuple + translation_xyz_m: tuple + rotation_uncertainty_rad: float + translation_uncertainty_m: float + training_stamps: tuple + validation_stamps: tuple + comparison_adjusted_p_values: tuple = () + + +def _projection(matrix, points): + pixels = points @ matrix.T + return pixels[:, :2]/pixels[:, 2:] + + +def _jacobian(function, steps): + zero = np.zeros(len(steps)) + result = [] + for index, step in enumerate(steps): + perturbation = zero.copy() + perturbation[index] = step + result.append((function(perturbation)-function(-perturbation)).ravel()/(2*step)) + return np.column_stack(result) + + +def _relative_covariance(fit, indices, parents, parent_corners, parent_size, child_points, matrices, variance): + """Propagate same-image parent-pixel uncertainty through the relative fit. + + Parent geometry is never changed. Image noise contributes per frame; + inherited geometry uncertainty is added separately without averaging it. + """ + information = np.zeros((6, 6)) + steps = np.r_[np.full(3, 1e-5), np.full(3, 1e-6)] + relative = Rotation.from_rotvec(fit.x[:3]) + local_child = relative.apply(child_points)+fit.x[3:] + for position, index in enumerate(indices): + parent, matrix = parents[index], matrices[index] + rotation = Rotation.from_quat(parent.quaternion_xyzw) + translation = np.asarray(parent.translation_xyz_m) + def project_parent(delta, points): + return _projection(matrix, rotation.apply(Rotation.from_rotvec(delta[:3]).apply(points)) + + translation+delta[3:]) + points = square_object_points(parent_size) + jacobian = _jacobian(lambda delta: project_parent(delta, points), steps) + _, singular, vh = np.linalg.svd(jacobian, full_matrices=False) + if singular[-1] <= singular[0]*np.finfo(float).eps*max(jacobian.shape): + raise ValueError('observer_parent_pose_rank_deficient') + error = project_parent(np.zeros(6), points)-parent_corners[index] + parent_variance = max(.03**2, float(np.sum(error**2)/2)) + parent_covariance = (vh.T/singular**2)@vh*parent_variance + inherited = _jacobian(lambda delta: project_parent(delta, local_child), steps) + pixel_covariance = np.eye(8)*variance + inherited@parent_covariance@inherited.T + relative_jacobian = fit.jac[position*8:(position+1)*8] + information += relative_jacobian.T@np.linalg.solve(pixel_covariance, relative_jacobian) + eigenvalues = np.linalg.eigvalsh(information) + if eigenvalues[0] <= eigenvalues[-1]*np.finfo(float).eps*6: + raise ValueError('observer_relative_pose_rank_deficient') + return np.linalg.inv(information) + + +def fit_observer_pose(parents, candidates, corners, matrices, size, stamps, *, inherited=(0., 0.), + parent_corners=None, parent_size=None): + count = len(stamps) + if count < 24 or tuple(stamps) != tuple(sorted(set(stamps))) or any(not c for c in candidates): + raise ValueError("observer_independent_images_missing") + training, validation = np.arange(0, count, 2), np.arange(1, count, 2) + # Bound work without selecting images by a fit's residual. + training = training[np.linspace(0, len(training)-1, min(72, len(training))).round().astype(int)] + validation = validation[np.linspace(0, len(validation)-1, min(72, len(validation))).round().astype(int)] + rotations = Rotation.from_quat([p.quaternion_xyzw for p in parents]) + for indices in (training, validation): + if max((rotations[indices[0]].inv()*rotations[indices]).magnitude()) < math.radians(5): + raise ValueError("observer_viewpoint_span_insufficient") + rotation_matrices = rotations.as_matrix() + translations = np.asarray([p.translation_xyz_m for p in parents]) + matrices, pixels = np.asarray(matrices), np.asarray(corners) + points = square_object_points(size) + + def residual(x, indices): + local = Rotation.from_rotvec(x[:3]).apply(points)+x[3:] + camera = np.einsum('nij,kj->nki', rotation_matrices[indices], local)+translations[indices, None, :] + if np.any(camera[..., 2] <= 0): + return np.full(len(indices)*8, 1e6) + projected = np.einsum('nij,nkj->nki', matrices[indices], camera) + return (projected[..., :2]/projected[..., 2:]-pixels[indices]).ravel() + + fits = [] + for pose in candidates[training[0]]: + rotation, point = _relative_pose(parents[training[0]], pose) + fit = least_squares(residual, np.r_[rotation.as_rotvec(), point], args=(training,), + x_scale=np.r_[np.ones(3), np.full(3, .03)], max_nfev=150, jac='3-point') + if not fit.success or not np.all(np.isfinite(fit.fun)): + raise ValueError("observer_candidate_not_evaluable") + fits.append(fit) + winner = min(fits, key=lambda f: float(f.fun @ f.fun)) + for indices in (training, validation): + errors = residual(winner.x, indices).reshape(-1, 4, 2) + if np.max(np.sqrt(np.mean(np.sum(errors**2, axis=2), axis=1))) > 1.5: + raise ValueError("observer_image_validation_failed") + from .image_model_selection import compare_image_losses, holm_adjusted_probabilities + def errors(fit, indices): + return np.sqrt(np.mean(np.sum(residual(fit.x, indices).reshape(-1, 4, 2)**2, axis=2), axis=1)) + comparisons = [] + angles = (rotations[0].inv()*rotations[validation]).as_rotvec() + for index, fit in enumerate(fits): + if fit is winner: + continue + difference = (Rotation.from_rotvec(winner.x[:3]).inv()*Rotation.from_rotvec(fit.x[:3])).magnitude() + equivalent = difference <= math.radians(.1) and np.linalg.norm(winner.x[3:]-fit.x[3:]) <= .0001 + probability = (0. if equivalent or np.max(errors(fit, training)) > 1.5 else + compare_image_losses(errors(winner, validation), errors(fit, validation), angles, .03)) + comparisons.append((index, probability)) + adjusted = holm_adjusted_probabilities(comparisons) + if any(probability > .01 for _, probability in adjusted): + raise ValueError('observer_candidates_unresolved') + _, singular, vh = np.linalg.svd(winner.jac, full_matrices=False) + if singular[-1] <= singular[0]*np.finfo(float).eps*max(winner.jac.shape): + raise ValueError("observer_pose_rank_deficient") + variance = max(.03**2, float(winner.fun@winner.fun/max(1, len(winner.fun)-6))) + covariance = (vh.T/singular**2)@vh*variance + if parent_corners is not None: + covariance = _relative_covariance(winner, training, parents, np.asarray(parent_corners), parent_size, + points, matrices, variance) + bounds = [float(3*np.sqrt(max(0., np.linalg.eigvalsh(covariance[i:i+3, i:i+3])[-1]))) + + inherited[i//3] for i in (0, 3)] + if not all(math.isfinite(v) for v in bounds) or bounds[0] > math.radians(1) or bounds[1] > .001: + raise ValueError("observer_pose_uncertain") + return ObserverPose(tuple(Rotation.from_rotvec(winner.x[:3]).as_quat()), tuple(winner.x[3:]), + *bounds, tuple(stamps[i] for i in training), tuple(stamps[i] for i in validation), adjusted) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py index 721824b..a9fba38 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py @@ -18,15 +18,26 @@ from .motion_image_types import ImageMotionFrame from .relative import _relative_pose SHARED_TAG_SELECTION_POLICY = "training_squared_loss_shared_pose_holm_v2" +OBSERVER_POSE_SELECTION_POLICY = "training_measured_observer_pose_holm_v1" SHARED_TAG_CONDITIONED_POLICY = "training_shared_zero_orientation_holm_v3" SHARED_ZERO_CONDITIONING = "shared_zero_orientation_v1" SHARED_POSE_CONDITIONING = "shared_zero_pose_v2" SHARED_TAG_POSE_CONDITIONED_POLICY = "training_shared_zero_pose_holm_v4" +SHARED_TAG_HANDOFF_POLICY = "training_shared_zero_pose_handoff_v1" SHARED_TAG_CONDITIONING_POLICIES = { SHARED_TAG_SELECTION_POLICY: None, SHARED_TAG_CONDITIONED_POLICY: SHARED_ZERO_CONDITIONING, SHARED_TAG_POSE_CONDITIONED_POLICY: SHARED_POSE_CONDITIONING, + SHARED_TAG_HANDOFF_POLICY: SHARED_POSE_CONDITIONING, } +OBSERVER_POSE_SELECTION_POLICIES = { + OBSERVER_POSE_SELECTION_POLICY: None, + **{f'{policy}_observer_v1': policy for policy in SHARED_TAG_CONDITIONING_POLICIES}, +} + + +def is_handoff_policy(policy): + return OBSERVER_POSE_SELECTION_POLICIES.get(policy, policy) == SHARED_TAG_HANDOFF_POLICY if TYPE_CHECKING: from .image_motion_model import ImageMotionModel @@ -40,9 +51,20 @@ class SharedTagPoseBridge: frames: tuple[ImageMotionFrame, ...] source_model: "ImageMotionModel | None" = None conditioning: str = SHARED_POSE_CONDITIONING + rotation_uncertainty_rad: float = 0. + translation_uncertainty_m: float = 0. + # Before the new joint moves, the source hinge verifies its own return. + # After that motion, only the recipient hinge explains the new images. + # Empty preserves the historical same-image authorization contract. + source_frames: tuple[ImageMotionFrame, ...] = () def shared_pose_selection_policy(bridges): + if any(b.source_frames for b in bridges): + if not all(b.source_frames and b.source_model is not None + and b.conditioning == SHARED_POSE_CONDITIONING for b in bridges): + raise ValueError("shared_pose_handoff_policy_mixed") + return SHARED_TAG_HANDOFF_POLICY modes = {b.conditioning for b in bridges if b.source_model is not None} if not modes: return SHARED_TAG_SELECTION_POLICY @@ -54,6 +76,14 @@ def shared_pose_selection_policy(bridges): raise ValueError("shared_pose_conditioning_policy_changed") +def observer_pose_selection_policy(shared_bridges): + if not shared_bridges: + return OBSERVER_POSE_SELECTION_POLICY + shared_policy = shared_pose_selection_policy(shared_bridges) + return next(policy for policy, source in OBSERVER_POSE_SELECTION_POLICIES.items() + if source == shared_policy) + + @dataclass(frozen=True) class PoseBridgeCheck: joint: str @@ -80,10 +110,14 @@ def check_source_pose(bridge): (bridge.relation.parent_role, bridge.relation.child_role)): return PoseBridgeCheck(bridge.relation.joint, "unresolved", "shared_pose_source_roles_changed") references = {ref.role: ref for ref in source.reference_frames} + source_frames = bridge.source_frames or bridge.frames + if bridge.source_frames and (not bridge.frames or + max(f.evidence.stamp_ns for f in source_frames) >= min(f.evidence.stamp_ns for f in bridge.frames)): + return PoseBridgeCheck(bridge.relation.joint, "unresolved", "shared_pose_handoff_order_invalid") frames = tuple(replace(frame, reference_frames=source.reference_frames, evidence=replace(frame.evidence, roles=tuple( RoleCandidates(role.role, (references[role.role].pose,), True) - if role.role in references else role for role in frame.evidence.roles))) for frame in bridge.frames) + if role.role in references else role for role in frame.evidence.roles))) for frame in source_frames) return check_pose_bridge(source, replace(bridge, relation=relation, frames=frames, source_model=None)) @@ -109,7 +143,7 @@ def check_pose_bridge(model, bridge): checked = select_image_motion_frame(model, frame) poses = {s.role: s.pose for s in checked.selections} if not checked.resolved: - return unknown("shared_pose_current_image_unresolved") + return unknown(f"shared_pose_current_image_unresolved:{checked.reason}:stamp={frame.evidence.stamp_ns}") try: rotation, point = _relative_pose(poses[bridge.relation.parent_role], poses[bridge.relation.child_role]) except KeyError: @@ -123,9 +157,13 @@ def check_pose_bridge(model, bridge): angles = (Rotation.from_quat(bridge.quaternion_xyzw).inv()*rotations).magnitude() distances = np.linalg.norm(points-np.asarray(bridge.translation_xyz_m), axis=1) rotation_limit, translation_limit = math.radians(2.), .005 - if np.max(angles) <= rotation_limit and np.max(distances) <= translation_limit: + angle_bound, point_bound = bridge.rotation_uncertainty_rad, bridge.translation_uncertainty_m + if (not all(math.isfinite(v) and v >= 0 for v in (angle_bound, point_bound)) + or angle_bound > math.radians(1.) or point_bound > .001): + return unknown("shared_pose_source_uncertainty_invalid") + if np.max(angles)+angle_bound <= rotation_limit and np.max(distances)+point_bound <= translation_limit: status, reason = "consistent", "" - elif np.min(angles) > 2*rotation_limit or np.min(distances) > 2*translation_limit: + elif np.min(angles)-angle_bound > 2*rotation_limit or np.min(distances)-point_bound > 2*translation_limit: status, reason = "conflicting", "shared_pose_outside_both_pose_margins" else: status, reason = "unresolved", "shared_pose_tolerance_regions_overlap" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py index cb83d42..0c6e6e1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py @@ -42,20 +42,37 @@ class ImageMotionParameters: maximum_hypotheses: int = 64 maximum_relations: int = 6 familywise_error_probability: float = .01 + report_pose_uncertainty: bool = False + maximum_solver_recovery_evaluations: int = 50 def __post_init__(self): + if (type(self.maximum_solver_recovery_evaluations) is not int + or not 0 <= self.maximum_solver_recovery_evaluations <= 100): + raise ValueError("maximum_solver_recovery_evaluations must be an integer in [0, 100]") for name in ("maximum_training_frames", "maximum_validation_frames", "maximum_solver_evaluations", "maximum_frame_evaluations", "minimum_frames", "maximum_hypotheses", "maximum_relations"): if type(getattr(self, name)) is not int or getattr(self, name) <= 0: raise ValueError(f"{name} must be a positive integer") if (self.minimum_frames < 24 or self.maximum_training_frames < 12 - or self.maximum_training_frames > 72 or self.maximum_validation_frames > 144 + or self.maximum_training_frames > 144 or self.maximum_validation_frames > 144 or self.maximum_solver_evaluations > 500 or self.maximum_frame_evaluations > 100 or self.maximum_hypotheses > 256 or self.maximum_relations > 8 or not 0 < self.familywise_error_probability <= .01): raise ValueError("image_motion_limits_invalid") +DENSE_PREPARATION_SAMPLING = "native_command_256_split144_v1" + + +def image_parameters_for_sampling(policy=None): + """Versioned sampling budgets, including historical journal replay.""" + if policy is None: + return ImageMotionParameters() + if policy == DENSE_PREPARATION_SAMPLING: + return ImageMotionParameters(maximum_training_frames=144, maximum_validation_frames=144) + raise ValueError("image_motion_sampling_policy_invalid") + + def _model(bundle, fit, frames, branches, training, validation): from .cad_hinge import CadImageHingeBundle model_type, extra = ImageMotionModel, {"reference_frames": frames[0].reference_frames} @@ -75,13 +92,18 @@ def _model(bundle, fit, frames, branches, training, validation): def _fit_hypothesis(frames, relations, poses, branches, training, validation, limits, - *, geometry_constraints=(), source_urdf_sha256="", source_hinges=(), pose_bridges=()): + *, geometry_constraints=(), source_urdf_sha256="", source_hinges=(), pose_bridges=(), + transition=None, transition_poses=None): if any(pose is None for role in poses.values() for pose in role): return ImageMotionHypothesis(branches, False, "incomplete_candidate_family"), None, None try: pnp = _pnp_diagnostics(frames, relations, poses) selected = tuple(frames[i] for i in training) - if pose_bridges: + if transition is not None: + from .transition_image_solver import TransitionImageHingeBundle + bundle = TransitionImageHingeBundle(selected, relations, _subset(poses, training), + transition, transition_poses, limits) + elif pose_bridges: from .shared_zero_solver import SharedZeroImageHingeBundle if geometry_constraints: raise ValueError("shared_zero_model_graph_unsupported") @@ -101,18 +123,24 @@ def _fit_hypothesis(frames, relations, poses, branches, training, validation, li training_quality = _quality(fit.fun, bundle.child_roles) training_converged = bool(fit.success and np.all(np.isfinite(fit.fun))) axes, pivots, child_bounds = hinge_uncertainty(bundle, fit) if training_converged else ((), (), ()) + pose_bounds = () + if limits.report_pose_uncertainty and training_converged: + from .hinge_uncertainty import preparation_pose_uncertainty + pose_bounds = preparation_pose_uncertainty(bundle, fit) transfer_checks = () transfer_reason = "" if source_hinges and training_converged: - from .measured_transfer import transfer_geometry_checks, bind_measured_transfer + from .measured_transfer import assess_transfer_geometry, bind_measured_transfer current_bounds = tuple((name, axis, dict(pivots)[name]) for name, axis in axes) - transfer_checks, inherited = transfer_geometry_checks(model.geometry, + transfer_checks, inherited, precision_accepted = assess_transfer_geometry(model.geometry, geometry_constraints, source_hinges, current_bounds) axes = tuple((name, value+inherited[name][0]) for name, value in axes) pivots = tuple((name, value+inherited[name][1]) for name, value in pivots) child_bounds = tuple((name, axis+inherited[name][0], point+inherited[name][1]) for name, axis, point in child_bounds) - if all(c.consistent for c in transfer_checks): + if not precision_accepted: + transfer_reason = "image_motion_geometry_uncertain" + elif all(c.consistent for c in transfer_checks): model = bind_measured_transfer(model, geometry_constraints, source_hinges, source_urdf_sha256, current_bounds) else: @@ -140,11 +168,14 @@ def _fit_hypothesis(frames, relations, poses, branches, training, validation, li reason = "image_motion_optimizer_unresolved" if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in validation_quality): reason = "image_motion_reprojection_failed" + attempts = getattr(fit, "image_solver_attempts", ()) hypothesis = ImageMotionHypothesis(branches, converged, reason, training_quality, validation_quality, tuple((r.joint, r.motion_span_rad) for r in pnp), axes, pivots, - training_solver_status=int(fit.status), training_solver_evaluations=int(fit.nfev), + training_solver_status=int(fit.status), + training_solver_evaluations=sum(a.evaluations for a in attempts) if attempts else int(fit.nfev), + training_solver_attempts=attempts, training_accepted=training_accepted, child_frame_uncertainty=child_bounds, - source_geometry_checks=transfer_checks) + source_geometry_checks=transfer_checks, preparation_pose_uncertainty=pose_bounds) rms = None if residuals is None else np.sqrt(np.mean(np.sum(residuals ** 2, axis=-1), axis=(1, 2))) return hypothesis, model, rms except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: @@ -163,6 +194,9 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra from .pose_bridge import check_source_pose conditioned = tuple(bridge for bridge in pose_bridges if bridge.source_model is not None) for bridge in conditioned: + if bridge.source_frames and (not frames or max(f.evidence.stamp_ns for f in bridge.source_frames) + >= min(f.evidence.stamp_ns for f in frames)): + return ImageMotionResolution(False, 'image_motion_shared_pose_source_unverified:shared_pose_handoff_order_invalid') checked = check_source_pose(bridge) if checked.status != "consistent": return ImageMotionResolution(False, "image_motion_shared_pose_source_unverified:" + checked.reason) @@ -192,6 +226,12 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra hypotheses.append(hypothesis) models.append(model) residuals.append(rms) + return select_image_hypotheses(frames, validation, hypotheses, models, residuals, limits, + pose_bridges=pose_bridges) + + +def select_image_hypotheses(frames, validation, hypotheses, models, residuals, limits, *, pose_bridges=()): + """One acceptance policy for independent and transition-conditioned fits.""" # Independent stationary evidence is an eligibility constraint, applied # before choosing by training error. Held-out arc scores cannot select a # different winner. Inconclusive bridge evidence never excludes a family. @@ -205,6 +245,8 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra hypotheses[index] = replace(hypotheses[index], training_accepted=False, reason=hypotheses[index].reason or "image_motion_shared_pose_conflict") winner = training_candidate_index(hypotheses) + if winner is None and any(h.training_solver_status == 0 for h in hypotheses): + return ImageMotionResolution(False, "image_motion_solver_budget_exhausted", hypotheses=tuple(hypotheses)) if winner is None and any(h.reason == "image_motion_shared_pose_conflict" for h in hypotheses): return ImageMotionResolution(False, "image_motion_shared_pose_conflict", hypotheses=tuple(hypotheses)) if winner is None or hypotheses[winner].reason: @@ -223,6 +265,8 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra continue if alternative is None or residuals[index] is None or not hypotheses[index].converged: # Missing candidate evidence is not evidence excluding a branch. + if hypotheses[index].training_solver_status == 0: + return ImageMotionResolution(False, "image_motion_solver_budget_exhausted", hypotheses=tuple(hypotheses)) return ImageMotionResolution(False, "image_motion_alternative_not_evaluable", hypotheses=tuple(hypotheses)) if any(check.status == "conflicting" for check in hypotheses[index].pose_bridge_checks): continue @@ -254,4 +298,5 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra final = select_image_motion_frame(model, frames[-1]) if not final.resolved: return ImageMotionResolution(False, final.reason, hypotheses=tuple(hypotheses)) - return ImageMotionResolution(True, "", model, tuple(hypotheses), final.selections) + return ImageMotionResolution(True, "", model, tuple(hypotheses), final.selections, + selected_hypothesis=winner) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py index 336839e..9bd1a40 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py @@ -8,13 +8,13 @@ run, and the independent arc holdout retains every ordinary image quality gate. """ import numpy as np -from scipy.optimize import least_squares from scipy.sparse import lil_matrix from scipy.spatial.transform import Rotation from .motion_image_solver import ImageHingeBundle from .relative import _relative_pose from .pose_bridge import SHARED_POSE_CONDITIONING, SHARED_ZERO_CONDITIONING +from .image_bundle_optimizer import solve_image_bundle class SharedZeroImageHingeBundle(ImageHingeBundle): @@ -86,9 +86,8 @@ class SharedZeroImageHingeBundle(ImageHingeBundle): rows = slice(frame*width+joint*8, frame*width+(joint+1)*8) sparsity[rows, self.geometry_parameter_indices(joint)] = 1 sparsity[rows, self.static_parameter_count+frame*self.joints+joint] = 1 - return least_squares(self.residual, self.initial, jac_sparsity=sparsity.tocsr(), - x_scale=self.scale, max_nfev=limits.maximum_solver_evaluations, - tr_options={"atol": 1e-10, "btol": 1e-10}, ftol=1e-9, xtol=1e-9, gtol=1e-7) + return solve_image_bundle(self.residual, self.initial, sparsity=sparsity.tocsr(), + scale=self.scale, limits=limits) def uncertainty_problem(self, fit): """Evaluate full image sensitivity without treating the anchor as exact. diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/transition_image_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/transition_image_solver.py new file mode 100644 index 0000000..b1d68cf --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/transition_image_solver.py @@ -0,0 +1,213 @@ +"""Joint image fit of an existing posture transition and the next preparation. + +The two arcs share their physically held endpoint. The earlier measured zero +anchors the first arc; SDK values identify held postures, never camera angles. +The transition is a nuisance motion, not a newly certified calibration joint. +All its images retain a separate, predetermined training/validation partition. +""" + +from dataclasses import dataclass, replace +from itertools import product + +import numpy as np +from scipy.optimize import least_squares +from .image_bundle_optimizer import solve_image_bundle +from scipy.sparse import lil_matrix +from scipy.spatial.transform import Rotation + +from .motion_image_solver import ImageHingeBundle +from .motion_image_types import ImageMotionFrame +from .motion_evidence import MotionRelation +from .motion_image_diagnostics import _sample, _subset, _validate_frames +from .relative import _relative_pose + + +TRANSITION_SELECTION_POLICY = "training_shared_transition_images_holm_v1" + + +@dataclass(frozen=True) +class PreparationTransition: + relation: MotionRelation + frames: tuple[ImageMotionFrame, ...] + endpoints: tuple[int, ...] # -1: moving; 0: earlier zero; 1: shared endpoint + quaternion_xyzw: tuple + translation_xyz_m: tuple + current_zero_stamps: tuple[int, ...] + + +class TransitionImageHingeBundle(ImageHingeBundle): + """Two measured hinges connected at a shared pose, with bounded work.""" + + def __init__(self, frames, relations, poses, transition, transition_poses, limits): + super().__init__(frames, relations, poses) + if len(relations) != 1: + raise ValueError("preparation_transition_requires_single_hinge") + original = self.initial.copy() + self.transition = transition + self.zero_rotation = Rotation.from_quat(transition.quaternion_xyzw) + self.zero_translation = np.asarray(transition.translation_xyz_m) + count = len(transition.frames) + self.aux_training = _sample(list(range(0, count, 2)), limits.maximum_training_frames) + self.aux_validation = _sample(list(range(1, count, 2)), limits.maximum_validation_frames) + self.aux_poses = transition_poses + self.aux = ImageHingeBundle(tuple(transition.frames[i] for i in self.aux_training), + (transition.relation,), _subset(transition_poses, self.aux_training)) + self.endpoints = np.asarray(transition.endpoints)[self.aux_training] + self.free_aux = np.flatnonzero(self.endpoints == -1) + zeros = set(transition.current_zero_stamps) + self.free_main = np.asarray([i for i, frame in enumerate(frames) + if frame.evidence.stamp_ns not in zeros], dtype=int) + parent, child = relations[0].parent_role, relations[0].child_role + ending = [i for i in self.aux_training if transition.endpoints[i] == 1][-1] + end_rotation, _ = _relative_pose(transition_poses[parent][ending], transition_poses[child][ending]) + end_angle = float((end_rotation*self.zero_rotation.inv()).as_rotvec() @ self.aux.axes[0]) + main_angles = [float((_relative_pose(p, c)[0]*end_rotation.inv()).as_rotvec() @ self.axes[0]) + for p, c in zip(poses[parent], poses[child])] + aux_angles = [float((_relative_pose(p, c)[0]*self.zero_rotation.inv()).as_rotvec() @ self.aux.axes[0]) + for p, c in zip(self.aux.poses[parent], self.aux.poses[child])] + self.initial = np.r_[0., 0., original[5:7], 0., 0., self.aux.initial[5:7], end_angle, + np.asarray(main_angles)[self.free_main], np.asarray(aux_angles)[self.free_aux]] + self.scale = np.r_[1., 1., .05, .05, 1., 1., .05, .05, 1., + np.ones(len(self.free_main)+len(self.free_aux))] + + @property + def static_parameter_count(self): + return 9 + + def auxiliary_geometry(self, parameters): + turn = Rotation.from_rotvec(self.aux.bases[0] @ parameters[4:6]) + axis = turn.apply(self.aux.axes[0]) + pivot = turn.apply(self.aux.bases[0] @ parameters[6:8]) + mount = self.zero_rotation.inv().apply(self.zero_translation-pivot) + return axis, pivot, mount + + def geometry_values(self, parameters, index): + axis, pivot, mount = self.auxiliary_geometry(parameters) + reference = Rotation.from_rotvec(axis*parameters[8])*self.zero_rotation + point = pivot+reference.apply(mount) + turn = Rotation.from_rotvec(self.bases[index] @ parameters[:2]) + current_pivot = turn.apply(self.bases[index] @ parameters[2:4]) + return turn.apply(self.axes[index]), reference, current_pivot, reference.inv().apply(point-current_pivot) + + def main_angles(self, parameters): + angles = np.zeros((self.count, 1)) + angles[self.free_main, 0] = parameters[9:9+len(self.free_main)] + return angles + + def auxiliary_pixels(self, parameters, angles, roots, matrices, objects): + axis, pivot, mount = self.auxiliary_geometry(parameters) + relative = Rotation.from_rotvec(np.asarray(angles)[:, None]*axis)*self.zero_rotation + parent_rotation, parent_translation = roots[self.relations[0].parent_role] + rotation = parent_rotation*relative + translation = parent_translation+parent_rotation.apply(pivot+relative.apply(mount)) + points = np.einsum('nij,kj->nki', rotation.as_matrix(), objects[self.child_roles[0]]) + points += translation[:, None, :] + pixels = np.einsum('nij,nkj->nki', matrices, points) + return pixels[:, :, :2]/np.maximum(pixels[:, :, 2:], 1e-6), bool(np.all(points[:, :, 2] > 1e-6)) + + def residual(self, parameters): + pixels, _ = self.project(parameters, self.main_angles(parameters), + self.root_transforms, self.matrices, self.objects) + angles = np.where(self.endpoints == 1, parameters[8], 0.) + angles[self.free_aux] = parameters[9+len(self.free_main):] + auxiliary, _ = self.auxiliary_pixels(parameters, angles, self.aux.root_transforms, + self.aux.matrices, self.aux.objects) + return np.r_[(pixels-self.observed).ravel(), (auxiliary-self.aux.observed[:, 0]).ravel()] + + def solve(self, limits): + sparsity = lil_matrix((8*(self.count+self.aux.count), len(self.initial)), dtype=int) + sparsity[:, :9] = 1 + for column, frame in enumerate(self.free_main): + sparsity[frame*8:(frame+1)*8, 9+column] = 1 + for column, frame in enumerate(self.free_aux): + start = (self.count+frame)*8 + sparsity[start:start+8, 9+len(self.free_main)+column] = 1 + return solve_image_bundle(self.residual, self.initial, sparsity=sparsity.tocsr(), + scale=self.scale, limits=limits) + + def validate(self, parameters, frames, poses, limits): + residuals, valid = super().validate(parameters, frames, poses, limits) + # Shared endpoint identity is a physical constraint, not a free angle. + roots, matrices, observed, objects = self._image_inputs(frames, poses) + zeros = set(self.transition.current_zero_stamps) + for i, frame in enumerate(frames): + if frame.evidence.stamp_ns in zeros: + frame_roots = {role: (r[i:i+1], t[i:i+1]) for role, (r, t) in roots.items()} + pixels, positive = self.project(parameters, np.zeros((1, 1)), frame_roots, + matrices[i:i+1], objects) + residuals[i] = (pixels[0]-observed[i]).reshape(1, 4, 2) + valid = valid and positive + selected = tuple(self.transition.frames[i] for i in self.aux_validation) + aux_poses = _subset(self.aux_poses, self.aux_validation) + roots, matrices, observed, objects = self.aux._image_inputs(selected, aux_poses) + auxiliary = [] + for position, index in enumerate(self.aux_validation): + frame_roots = {role: (r[position:position+1], t[position:position+1]) + for role, (r, t) in roots.items()} + def error(theta): + pixels, _ = self.auxiliary_pixels(parameters, theta, frame_roots, + matrices[position:position+1], objects) + return (pixels[0]-observed[position, 0]).ravel() + mode = self.transition.endpoints[index] + if mode >= 0: + theta = [parameters[8] if mode == 1 else 0.] + else: + relative, _ = _relative_pose(aux_poses[self.relations[0].parent_role][position], + aux_poses[self.child_roles[0]][position]) + axis, _, _ = self.auxiliary_geometry(parameters) + initial = float((relative*self.zero_rotation.inv()).as_rotvec() @ axis) + fit = least_squares(error, [initial], max_nfev=limits.maximum_frame_evaluations, + ftol=1e-9, xtol=1e-9, gtol=1e-7) + theta = fit.x + valid = valid and bool(fit.success) + _, positive = self.auxiliary_pixels(parameters, theta, frame_roots, + matrices[position:position+1], objects) + valid = valid and positive + auxiliary.append(error(theta).reshape(1, 4, 2)) + return np.concatenate((residuals, auxiliary)), valid + + def uncertainty_problem(self, fit): + # As for a shared-zero fit, restore every target geometry freedom. + # Neither the old anchor nor repeated transition images may manufacture + # precise current geometry by removing uncertain variables. + from .hinge_uncertainty import unconstrained_hinge_linearization + return unconstrained_hinge_linearization(self, fit, self.main_angles(fit.x)) + + +def resolve_transition_motion(frames, relations, transition, *, parameters=None): + from .production_image_motion import ImageMotionParameters, _fit_hypothesis, select_image_hypotheses + from .image_motion_model import ImageMotionResolution + limits = parameters or ImageMotionParameters() + try: + paths, roles = _validate_frames(frames, relations, limits) + auxiliary, auxiliary_roles = _validate_frames(transition.frames, (transition.relation,), limits) + if (len(relations) != 1 or roles != auxiliary_roles + or len(transition.frames) != len(transition.endpoints) + or any(value not in {-1, 0, 1} for value in transition.endpoints) + or 0 not in transition.endpoints or transition.endpoints.count(1) < 10 + or not transition.current_zero_stamps + or not set(transition.current_zero_stamps) <= {f.evidence.stamp_ns for f in frames} + or max(f.evidence.stamp_ns for f in transition.frames) >= frames[0].evidence.stamp_ns + or any(f.camera_matrix != frames[0].camera_matrix + or f.reference_frames != frames[0].reference_frames for f in transition.frames)): + raise ValueError("preparation_transition_support_invalid") + training = _sample(list(range(0, len(frames), 2)), limits.maximum_training_frames) + validation = _sample(list(range(1, len(frames), 2)), limits.maximum_validation_frames) + families = np.prod([len(paths[r])*len(auxiliary[r]) for r in roles]) + if families > limits.maximum_hypotheses: + raise ValueError("preparation_transition_hypothesis_limit") + except (ValueError, TypeError, KeyError) as error: + return ImageMotionResolution(False, str(error)) + hypotheses, models, residuals = [], [], [] + for indices in product(*(range(len(paths[r])) for r in roles)): + branches = tuple(zip(roles, indices)) + poses = {role: paths[role][i] for role, i in branches} + for choices in product(*(range(len(auxiliary[r])) for r in roles)): + other = tuple(zip(roles, choices)) + auxiliary_poses = {role: auxiliary[role][i] for role, i in other} + hypothesis, model, errors = _fit_hypothesis(frames, relations, poses, branches, + training, validation, limits, transition=transition, transition_poses=auxiliary_poses) + hypotheses.append(replace(hypothesis, transition_branches=other)) + models.append(model) + residuals.append(errors) + return select_image_hypotheses(frames, validation, hypotheses, models, residuals, limits) diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py index b4ce4f0..9483ee8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py @@ -60,6 +60,8 @@ def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]: "joint_coverage": _plain(profile.joint_coverage), } payload["quality"].pop("task_training_cycles") + if not profile.motion.preparation_witnesses: + payload["motion"].pop("preparation_witnesses") return payload diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py index 9cbb61f..93fb576 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py @@ -287,6 +287,9 @@ def load_hand_profile(path: str | Path, *, scope: str | None = None) -> Calibrat tuple(tuple(map(float, command)) for command in row.get("approach_commands", ())), str(row.get("evidence_scope", "arrival"))) for name, row in motion.get("joint_zero_references", {}).items()}, + {str(name): ModelPreparation(str(item["joint"]), tuple(map(float, item["command"])), + tuple(tuple(map(float, pose)) for pose in item["approach_commands"])) + for name, item in motion.get("preparation_witnesses", {}).items()}, ), measurement=MeasurementPolicy( measurements, diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/preparation.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/preparation.py new file mode 100644 index 0000000..d26f547 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/preparation.py @@ -0,0 +1,71 @@ +"""Explicit, bounded pose excitation at each task's existing clearance pose.""" +from dataclasses import replace + +from ..core.domain.profile import ModelPreparation + + +def preparation_image_budget(profile, *, joint_count=1): + """Retain native command resolution for the O30 staged preparation plan. + + A parent hinge spends precision again when transferred to a child. Keep + more of the already acquired arc, with separate, bounded training and + validation halves. This is selected before fitting, never after a failure. + """ + from ..core.geometry.tag_pose.production_image_motion import ( + DENSE_PREPARATION_SAMPLING, image_parameters_for_sampling, + ) + dense = (profile.key.model == 'O30' + and profile.quality.training_policy == 'staged_2_to_3' + and bool(profile.motion.preparation_witnesses) and joint_count == 1) + return ((256, image_parameters_for_sampling(DENSE_PREPARATION_SAMPLING)) if dense + else (128, image_parameters_for_sampling())) + + +def configure_preparation(profile, policy): + if policy != 'occlusion_aware_witness_v1': + raise ValueError('unknown_preparation_policy') + if profile.key.profile_id != 'O30/right/o30_right_18/v1' or profile.quality.training_policy != 'staged_2_to_3': + raise ValueError('preparation_witness_requires_o30_staged_session') + from ..core.fitting.motion_fit import channel_for_joint + witnesses = {} + for finger in ('pinky', 'ring', 'middle', 'index'): + target, donor = f'{finger}_mcp_pitch', f'{finger}_mcp_roll' + command = profile.motion.joint_zero_references[target].command + channel = channel_for_joint(profile, donor) + lower, upper = profile.command.minimum_values[channel], profile.command.maximum_values[channel] + excursion = list(command) + # At most one quarter of the declared range, within native limits. + # Keep the other fingers in this task's already declared avoidance pose. + step = int((upper-lower)/4) # SDK u8 commands must survive rounding unchanged. + excursion[channel] = command[channel]-step if command[channel]-step >= lower else command[channel]+step + witnesses[target] = ModelPreparation(donor, command, (command, tuple(excursion))) + return replace(profile, motion=replace(profile.motion, preparation_witnesses=witnesses)) + + +def validate_preparation_witnesses(profile, source_model): + from .observations import compile_tag_feedback_channels + from ..core.fitting.motion_fit import channel_for_joint + channels = compile_tag_feedback_channels(profile, source_model) + tasks = {joint: task for task in profile.motion.tasks for joint in task.joints} + for target, witness in profile.motion.preparation_witnesses.items(): + donor = witness.joint + if target not in tasks or donor not in tasks or len(tasks[target].joints) != 1: + raise ValueError('preparation_witness_dependency_invalid') + spec = profile.measurement.measurements[target] + donor_channel = channel_for_joint(profile, donor) + moving = channels[spec.child_role]-channels[spec.parent_role] + if (donor_channel not in moving or donor_channel == channel_for_joint(profile, target) + or witness.command != profile.motion.joint_zero_references[target].command + or len(witness.approach_commands) != 2 or witness.approach_commands[0] != witness.command + or not any(tag.role == spec.parent_role and tag.fixed_reference + for view in profile.vision.views if view.name == spec.view for tag in view.tags)): + raise ValueError('preparation_witness_posture_not_shared') + for pose in (*witness.approach_commands, witness.command): + if (len(pose) != profile.command.command_count + or any(not lower <= value <= upper for value, lower, upper in zip(pose, + profile.command.minimum_values, profile.command.maximum_values)) + or any(value != witness.command[i] for i, value in enumerate(pose) if i != donor_channel)): + raise ValueError('preparation_witness_held_command_changed') + excursion = abs(witness.approach_commands[1][donor_channel]-witness.command[donor_channel]) + if not 0 < excursion <= (profile.command.maximum_values[donor_channel]-profile.command.minimum_values[donor_channel])/4: + raise ValueError('preparation_witness_excursion_outside_bound') diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py index 9bc6398..e5dbc49 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py @@ -25,6 +25,8 @@ def validate_executable_profile(profile, source_urdf): if any(sum(tag.fixed_reference for tag in view.tags) != 1 for view in profile.vision.views): raise ValueError("each capture view must declare exactly one fixed reference Tag") model = UrdfKinematicModel(source_urdf) + from .preparation import validate_preparation_witnesses + validate_preparation_witnesses(profile, model) spatial = compile_spatial_profile(profile) links = compile_tag_links(profile, model) _validate_parent_references(profile, model) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py index e77f687..0f825f0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py @@ -29,6 +29,7 @@ class FinalizationInputs: journal_path: Path | None = None journal_size: int | None = None camera_extrinsics_file: Path | None = None + frozen_training_sha256: str | None = None class FinalizationProtocolError(RuntimeError): @@ -56,6 +57,8 @@ class FinalizationController: transport = {} if inputs.camera_extrinsics_file is not None: transport["camera_extrinsics_file"] = inputs.camera_extrinsics_file + if inputs.frozen_training_sha256 is not None: + transport["frozen_training_sha256"] = inputs.frozen_training_sha256 if self._isolated: if inputs.journal_path is None or inputs.journal_size is None: raise ValueError("isolated_finalization_requires_synced_journal") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py index 61a0ad9..655215c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py @@ -92,6 +92,7 @@ def prepare_tag_replay( plan: StandardUrdfPlan, output_mappings: Mapping[str, JointMapping], common_from_base, records: Sequence[Mapping[str, Any]], directions_are_task_relative: bool = False, + frozen_training=None, ) -> TagReplayEvidence: """Use fit results for training poses; final validation reads files anew. @@ -151,6 +152,8 @@ def prepare_tag_replay( if cycle == profile.quality.holdout_cycle: holdout.append(TagHoldout(identity, role, cycle, sdk, directions, pose)) continue + if frozen_training is not None: + continue angles = evaluate_mappings(output_mappings, sdk, directions) # Resolve the FITTED linear relation in output coordinates, then # transform every joint back to CAD exactly once. Using source-CAD @@ -171,6 +174,11 @@ def prepare_tag_replay( cad = {joint: angle + plan.zero_offsets_rad.get(joint, 0.0) for joint, angle in angles.items()} training.append(TagTrainingPose(identity, role, cycle, pose, cad)) selected_links = {role: links[role] for role in required} + if frozen_training is not None: + mounts = frozen_training["tag_installations"] + if set(mounts) != required: + raise ValueError("frozen_tag_roles_changed") + return TagReplayEvidence(frozen_training["common_from_base"], mounts, tuple(holdout), tuple(sorted(required))) registered_base = register_base_translation(source_model=source_model, common_from_base=common_from_base, link_by_role=selected_links, observations=training) mounts = fit_tag_installations(source_model=source_model, common_from_base=registered_base, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py index 79a55d4..c523b76 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py @@ -7,7 +7,7 @@ from pathlib import Path from typing import Callable from ...core.artifacts.storage import atomic_write_json -from ...core.domain.capture_plan import CapturePlan, training_cycles +from ...core.domain.capture_plan import CapturePlan, STAGED, evidence_digest, training_cycles from ...core.fitting.session import fit_profile_calibration from ...core.fitting.command_mapping import fit_command_mappings from ...core.fitting.motion_fit import channel_for_joint @@ -64,6 +64,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf directions_are_task_relative: bool = False, require_motion_evidence: bool = False, camera_extrinsics_file=None, + frozen_training_sha256=None, phase_changed: Callable[[str], None] = lambda _phase: None): """No worker may publish after an operator abort. @@ -85,7 +86,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf check_cancelled() from ..training import resolve_capture_plan profile = resolve_capture_plan(profile, records, source_urdf=source, - verify_decisions=True, require_complete=True) + verify_decisions=frozen_training_sha256 is None, require_complete=True) from ..motion_provenance import uses_motion_evidence capture_evidence_required = (require_motion_evidence or uses_motion_evidence(records) or any(row.get("kind") == "session_start" for row in records)) @@ -144,6 +145,16 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf try: from ...core.domain.reference import read_joint_zero_references references = read_joint_zero_references(profile, records) if profile.artifacts.output_schema_version >= 3 else None + frozen_training = None + if profile.quality.training_policy == STAGED: + import json + from ..staged_training import read_frozen_training, staged_snapshot + payload = json.loads((directory / "frozen_training.json").read_text()) + decisions = [r for r in records if r.get("kind") == "staged_training_decision" and r.get("decision") == "freeze"] + if (len(decisions) != 1 or evidence_digest(payload) != decisions[0]["frozen_training_sha256"] + or (frozen_training_sha256 is not None and evidence_digest(payload) != frozen_training_sha256)): + raise ValueError("frozen_hand_training_digest_changed") + frozen_training = read_frozen_training(profile, staged_snapshot(profile, records), references, source, payload) def save_relative_motion(motion): if profile.artifacts.output_schema_version >= 3: atomic_write_json(directory / "relative_motion_diagnostics.json", { @@ -153,7 +164,8 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf steady = accepted_joint_records(profile, records, sample_phase="steady", directions_are_task_relative=directions_are_task_relative) fit = fit_profile_calibration(profile, source, accepted, cross_view_records=secondary, - zero_references=references, steady_records=steady, motion_fitted=save_relative_motion) + zero_references=references, steady_records=steady, motion_fitted=save_relative_motion, + frozen_training=frozen_training) if not profile.command_based_release: fit = fit_command_mappings(profile, fit, steady) if fit.feedback_diagnostics: @@ -190,7 +202,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf replay = prepare_tag_replay(profile=profile, source_model=UrdfKinematicModel(source), plan=prepared.plan, output_mappings=fit.output_mappings, common_from_base=transform_matrix(zero.base_translation_xyz_m, zero.base_quaternion_xyzw), - records=replay_records) + records=replay_records, frozen_training=frozen_training) except TagInstallationFailure as error: atomic_write_json(directory / "tag_installation_diagnostics.json", { "passed": False, "publication_allowed": False, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py index 6e7cd75..7949c85 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py @@ -22,6 +22,10 @@ def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool else: output = output.expanduser().resolve() output.mkdir(parents=True, exist_ok=False) + from ...core.domain.capture_plan import STAGED + if profile.quality.training_policy == STAGED: + import shutil + shutil.copyfile(raw_path.parent / "frozen_training.json", output / "frozen_training.json") payload, _fit, correction = finalize_profile_session( profile=profile, session_dir=output, serial_number=config.serial_number, source_urdf=config.source_urdf, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py index e7e9b13..24d70ca 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py @@ -6,7 +6,7 @@ outside that lock. Nothing here moves hardware, fits a control curve, or uses training/holdout observations to revise an existing reference. """ -from dataclasses import asdict, dataclass +from dataclasses import asdict, dataclass, replace from collections import deque import hashlib import json @@ -24,7 +24,7 @@ from ..core.geometry.tag_pose.pose_bridge import ( SharedTagPoseBridge, shared_pose_selection_policy, ) from ..core.geometry.tag_pose.production_image_motion import ( - ImageMotionModel, ImageMotionResolution, resolve_image_motion, + ImageMotionModel, ImageMotionResolution, ImageMotionParameters, resolve_image_motion, ) from .scan_quality import observation_streams from .motion_provenance import source_frame_sha256 @@ -64,6 +64,14 @@ class BranchInitializationRequest: pose_bridges: tuple[SharedTagPoseBridge, ...] = () pose_bridge_records: tuple[dict, ...] = () rejected_observations: tuple[tuple[str, int], ...] = () + observer_sources: tuple = () + observer_profile: object = None + transition_source: object = None + transition_profile: object = None + current_source_rows: tuple = () + witness_sources: tuple = () + witness_profile: object = None + image_parameters: ImageMotionParameters | None = None def _pose(payload): @@ -93,9 +101,57 @@ def solve_initialization(request: BranchInitializationRequest): if request.invalid_reason: return MotionBranchResolution(False, request.invalid_reason) if request.image_geometry: - return resolve_image_motion(request.image_frames, request.relations, + if request.witness_sources and request.transition_source is not None: + return ImageMotionResolution(False, 'preparation_witness_composition_unsupported') + transition_rejections = () + if request.transition_source is not None: + from .preparation_transition import build_preparation_transition, PreparationTransitionUnavailable + from ..core.geometry.tag_pose.transition_image_solver import resolve_transition_motion + if request.pose_bridges or request.geometry_constraints or request.source_hinges or request.observer_sources: + return ImageMotionResolution(False, 'preparation_transition_constraints_unsupported') + try: + transition, record = build_preparation_transition(request.transition_profile, + request.transition_source, request.current_source_rows) + except PreparationTransitionUnavailable as error: + # Decline missing optional data before training. A completed + # transition fit or holdout failure never falls back to another + # model-selection attempt. + transition_rejections = (str(error),) + except (ValueError, KeyError, TypeError, IndexError) as error: + return ImageMotionResolution(False, str(error)) + else: + result = resolve_transition_motion(request.image_frames, request.relations, transition, + parameters=request.image_parameters) + return replace(result, preparation_transitions=(record,)) + bridges, records, rejections = [], [], [] + if request.observer_sources: + from .observer_reference import build_observer_bridge + for source in request.observer_sources: + try: + bridge, record = build_observer_bridge(request.observer_profile, source) + bridges.append(bridge) + records.append(record) + except (ValueError, KeyError, TypeError, IndexError) as error: + rejections.append(str(error)) + witness_bridges, witness_records = [], [] + if request.witness_sources: + from .preparation_witness import build_preparation_witness + if request.pose_bridges or request.observer_sources or request.geometry_constraints or request.source_hinges: + return ImageMotionResolution(False, 'preparation_witness_composition_unsupported') + for source in request.witness_sources: + try: + bridge, record = build_preparation_witness(request.witness_profile, source) + if bridge is not None: + witness_bridges.append(bridge) + witness_records.append(record) + except (ValueError, KeyError, TypeError, IndexError) as error: + return ImageMotionResolution(False, str(error)) + result = resolve_image_motion(request.image_frames, request.relations, + parameters=request.image_parameters, geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256, - source_hinges=request.source_hinges, pose_bridges=request.pose_bridges) + source_hinges=request.source_hinges, pose_bridges=request.pose_bridges+tuple(bridges)+tuple(witness_bridges)) + return replace(result, observer_pose_bridges=tuple(records), observer_rejections=tuple(rejections), + preparation_transition_rejections=transition_rejections, preparation_witnesses=tuple(witness_records)) return resolve_motion_branches(request.frames, request.relations) @@ -126,16 +182,53 @@ def initialization_artifacts(request, resolution): record["frozen_image_model"] = payload record["image_model_sha256"] = hashlib.sha256(json.dumps( payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() - winner = next(item for item in resolution.hypotheses if item.branches == image_model.branches) + winner = (resolution.hypotheses[resolution.selected_hypothesis] + if resolution.selected_hypothesis is not None else + next(item for item in resolution.hypotheses if item.branches == image_model.branches)) record["geometry_uncertainty"] = [list(item) for item in winner.child_frame_uncertainty] if request.parent_reference_sha256: record["parent_reference_sha256"] = request.parent_reference_sha256 if request.pose_bridge_records: record["shared_tag_pose_bridges"] = list(request.pose_bridge_records) + if isinstance(resolution, ImageMotionResolution) and resolution.observer_pose_bridges: + record["observer_pose_bridges"] = list(resolution.observer_pose_bridges) + if isinstance(resolution, ImageMotionResolution) and resolution.observer_rejections: + record["observer_rejections"] = list(resolution.observer_rejections) + if isinstance(resolution, ImageMotionResolution) and resolution.preparation_transitions: + record["preparation_transitions"] = list(resolution.preparation_transitions) + record["selected_hypothesis"] = resolution.selected_hypothesis + if isinstance(resolution, ImageMotionResolution) and resolution.preparation_transition_rejections: + record["preparation_transition_rejections"] = list(resolution.preparation_transition_rejections) + if isinstance(resolution, ImageMotionResolution) and resolution.preparation_witnesses: + record["preparation_witnesses"] = list(resolution.preparation_witnesses) + record["selected_hypothesis"] = resolution.selected_hypothesis if request.image_geometry: + from ..core.geometry.tag_pose.production_image_motion import ( + DENSE_PREPARATION_SAMPLING, image_parameters_for_sampling, + ) + if request.image_parameters == image_parameters_for_sampling(DENSE_PREPARATION_SAMPLING): + record["image_sampling_policy"] = DENSE_PREPARATION_SAMPLING + if any(len(getattr(h, "training_solver_attempts", ())) > 1 for h in resolution.hypotheses): + from ..core.geometry.tag_pose.image_bundle_optimizer import IMAGE_BUNDLE_SOLVER_POLICY + record["image_solver_policy"] = IMAGE_BUNDLE_SOLVER_POLICY + record["image_solver_attempts"] = [ + {key: hypothesis[key] for key in ("branches", "training_solver_attempts", + "training_solver_status", "training_solver_evaluations")} + for hypothesis in record["hypotheses"]] record["image_model_selection_policy"] = IMAGE_MODEL_SELECTION_POLICY if request.pose_bridges: record["image_model_selection_policy"] = shared_pose_selection_policy(request.pose_bridges) + if record.get("observer_pose_bridges"): + from ..core.geometry.tag_pose.pose_bridge import observer_pose_selection_policy + record["image_model_selection_policy"] = observer_pose_selection_policy(request.pose_bridges) + if record.get("preparation_transitions"): + from ..core.geometry.tag_pose.transition_image_solver import TRANSITION_SELECTION_POLICY + record["image_model_selection_policy"] = TRANSITION_SELECTION_POLICY + if record.get("preparation_witnesses"): + from .preparation_witness import WITNESS_POLICY, INDEPENDENT_WITNESS_POLICY + record["image_model_selection_policy"] = (INDEPENDENT_WITNESS_POLICY + if all(r.get('decision') == 'independent_before_fit' for r in record['preparation_witnesses']) + else WITNESS_POLICY) record["image_geometry_constraints"] = [asdict(item) for item in request.geometry_constraints] record["source_urdf_sha256"] = request.source_urdf_sha256 if not resolution.resolved: @@ -155,10 +248,24 @@ def initialization_artifacts(request, resolution): if image_model is not None: payload.update(frozen_image_model=record["frozen_image_model"], image_model_sha256=record["image_model_sha256"], geometry_uncertainty=record["geometry_uncertainty"]) + if "image_solver_policy" in record: + payload.update(image_solver_policy=record["image_solver_policy"], + image_solver_attempts=record["image_solver_attempts"]) + if "image_sampling_policy" in record: + payload["image_sampling_policy"] = record["image_sampling_policy"] if request.parent_reference_sha256: payload["parent_reference_sha256"] = request.parent_reference_sha256 if request.pose_bridge_records: payload["shared_tag_pose_bridges"] = record["shared_tag_pose_bridges"] + from ..core.geometry.tag_pose.pose_bridge import is_handoff_policy + if is_handoff_policy(record['image_model_selection_policy']): + payload['image_model_selection_policy'] = record['image_model_selection_policy'] + if record.get("observer_pose_bridges"): + payload["observer_pose_bridges"] = record["observer_pose_bridges"] + if record.get("preparation_transitions"): + payload["preparation_transitions"] = record["preparation_transitions"] + if record.get("preparation_witnesses"): + payload["preparation_witnesses"] = record["preparation_witnesses"] payload["tag_role"] = role encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) record["evidence_ids"][role] = hashlib.sha256(encoded.encode()).hexdigest() @@ -179,6 +286,8 @@ class BranchInitialization: def __init__(self, profile, *, image_geometry=True, source_urdf=None): self.profile = profile self.image_geometry = image_geometry + from ..profiles.preparation import preparation_image_budget + self.command_intervals, self.image_parameters = preparation_image_budget(profile) self.source_model = None self.source_urdf_sha256 = "" self.tag_feedback_channels = None @@ -191,9 +300,18 @@ class BranchInitialization: self.generation = 0 from .parent_reference import ParentReferenceRegistry self.parent_references = ParentReferenceRegistry(profile, self.tag_feedback_channels) + self.observer_records = {} + from .preparation_transition import PreparationTransitionCapture + self.transition_capture = PreparationTransitionCapture(self) + from .preparation_witness import PreparationWitnessCapture + self.witness_capture = PreparationWitnessCapture(self) self.clear() def clear(self): + self.transition_capture.clear() + self._clear_motion() + + def _clear_motion(self): self.generation += 1 self.task_name = None self.zero_joints = () @@ -212,10 +330,17 @@ class BranchInitialization: self.path_index = None def begin(self, motion, *, session_epoch, motion_version): - if motion.reference_reuse: + if motion.preparation_witness_joint: + self.clear() + return + self.transition_capture.begin(motion, session_epoch) + if motion.reference_reuse or motion.reference_check: self.clear() return if motion.phase == "zero_approach": + from ..profiles.preparation import preparation_image_budget + self.command_intervals, self.image_parameters = preparation_image_budget( + self.profile, joint_count=len(motion.reference_joints)) index = motion.reference_path_index identity = (motion.task_key, motion.reference_joints, session_epoch) previous = (self.task_name, self.zero_joints, self.session_epoch) @@ -223,7 +348,7 @@ class BranchInitialization: and self.path_index is not None and index in {self.path_index, self.path_index + 1} and self.motion_version is not None and motion_version > self.motion_version) if not continuing: - self.clear() + self._clear_motion() if index is not None and index != 0: self._invalid_reason = "motion_approach_path_discontinuous" self.path_index = index @@ -235,7 +360,7 @@ class BranchInitialization: elif motion.phase == "joint_zero": if (self.task_name, self.zero_joints, self.session_epoch) != ( motion.task_key, motion.zero_joints, session_epoch): - self.clear() + self._clear_motion() self.task_name, self.zero_joints = motion.task_key, motion.zero_joints self.session_epoch = session_epoch self._invalid_reason = "motion_approach_evidence_missing" @@ -256,6 +381,10 @@ class BranchInitialization: return tuple(relations) def observe(self, record): + self.witness_capture.observe(record) + if record.get('preparation_witness_joint'): + return + self.transition_capture.observe(record) if (record["task_name"], tuple(record["zero_joints"]), record["motion_version"], record["session_epoch"]) != ( self.task_name, self.zero_joints, self.motion_version, self.session_epoch): @@ -334,12 +463,17 @@ class BranchInitialization: return existing.update(frame_evidence_ids) lower, upper = layout.minimum_values[index], layout.maximum_values[index] - bin_index = round(128 * (command[index] - lower) / (upper - lower)) + # Preserve native u8 command positions instead of discarding roughly + # half of an already recorded approach. There is still only one image + # per interval; stationary repeats cannot inflate the sample count. + bin_index = round(self.command_intervals * (command[index] - lower) / (upper - lower)) # A dropout must not erase an earlier eligible image of this same # motion interval. Every retained image still needs full model and # held-out validation; admission itself does not authorize a branch. self._frames.setdefault(record["view"], {})[bin_index] = frame self._source_records.setdefault(record["view"], {})[bin_index] = record + self.observer_records[(self.session_epoch, self.task_name, record['view'])] = { + row['image_stamp_ns']: row for row in self._source_records[record['view']].values()} # Keep a separate bounded stationary tail. Repeated endpoint images # must not inflate the arc's motion bins or enter its holdout split. tail = self._endpoint_records.setdefault(record['view'], deque( @@ -351,7 +485,7 @@ class BranchInitialization: tail.clear() def request(self, view, motion, *, session_epoch, motion_version, zero_references=None): - if motion is None or motion.reference_reuse or motion.phase != "joint_zero" or view in self._requested: + if motion is None or motion.reference_reuse or motion.reference_check or motion.phase != "joint_zero" or view in self._requested: return None if (self.task_name, self.zero_joints, self.session_epoch) != ( motion.task_key, motion.zero_joints, session_epoch): @@ -400,6 +534,17 @@ class BranchInitialization: from ..profiles.observations import compile_parallel_axis_geometry geometry_constraints = compile_parallel_axis_geometry(self.profile, self.source_model, tuple(item.geometry.relation for item in source_hinges) + relations) + from .observer_reference import observer_inputs + observer_sources = observer_inputs(self, view, self._endpoint_records.get(view, ()), + {f.stamp_ns for f in frames}, session_epoch) if self.image_geometry else () + transition_source = None + if self.image_geometry: + try: + transition_source = self.transition_capture.snapshot(view) + except ValueError as error: + reason = reason or str(error) + witness_sources = self.witness_capture.inputs(view, self.zero_joints, + self._endpoint_records.get(view, ()), {f.stamp_ns for f in frames}, session_epoch) if self.image_geometry else () return BranchInitializationRequest(self.task_name, view, self.zero_joints, session_epoch, self.motion_version if self.motion_version is not None else motion_version, self.generation, frames, @@ -410,7 +555,11 @@ class BranchInitialization: tuple(sorted((str(row["image_stamp_ns"]), row["motion_version"]) for row in sources)) if self.path_index is not None else (), source_hinges, parent_reference_sha256, pose_bridges, pose_bridge_records, - tuple(sorted(self._rejected_observations.get(view, {}).items()))) + tuple(sorted(self._rejected_observations.get(view, {}).items())), observer_sources, + self.profile if observer_sources else None, transition_source, + self.profile if transition_source is not None else None, + tuple(sorted(sources, key=lambda row: row['image_stamp_ns'])) if transition_source is not None else (), + witness_sources, self.profile if witness_sources else None, self.image_parameters) def pending(self): """A bounded preparation solve is running outside the state lock.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py index 13de8e9..1597047 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -117,7 +117,7 @@ class ObservationCapture: frozen joint zeros remain immutable; dependent image models are dropped together, so an old child can never use a newly interpreted parent. """ - if self.profile.acquisition.motion_model_scope != "task" or motion.phase != "zero_approach": + if self.profile.acquisition.motion_model_scope != "task" or motion.phase != "zero_approach" or motion.reference_check: return identity = (motion.task_key, motion.reference_joints) with self._metadata_lock: @@ -344,6 +344,11 @@ class ObservationCapture: tags = tuple(tag for tag in view.tags if not fixed_reference_only or tag.fixed_reference) if self.profile.acquisition.motion_model_scope == "task" and (motion is not None or self._motion_models): relevant = self._observed_roles(motion) + if motion is not None: + from .observer_reference import observer_roles + relevant.update(observer_roles(self.profile, motion)) + from .preparation_witness import witness_roles + relevant.update(witness_roles(self.profile, motion)) relevant.update(role for model in models for role, _ in model.evidence_ids) relevant.update(g.relation.parent_role for model in models for g in ( model.constraints if model.image_model is None else model.image_model.geometry)) @@ -404,12 +409,15 @@ class ObservationCapture: stamp_ns=frame.stamp_ns)[tag.role] diagnostics = item["candidate_diagnostics"] if diagnostics.get("observation_stamp_ns") != frame.stamp_ns: - pose, reason = None, "pose_branch_observation_superseded" + # An early model-commit rejection may have no committed + # snapshot. Keep its actual cause; missing metadata alone + # does not prove that a newer camera frame superseded it. + pose, reason = None, reason or "pose_branch_observation_superseded" elif pose is not None and diagnostics.get("branch_status") != "tracking": pose, reason = None, "pose_branch_unconfirmed" evidence[tag.role] = item tracking_rejected = pose is None and ( - reason.startswith(("pose_branch", "image_reference")) or reason == "fixed_reference_pose_unverified") + reason.startswith(("pose_", "image_reference")) or reason == "fixed_reference_pose_unverified") if pose is None: selected.pop(tag.role, None) filtered.append(tag.tag_id) @@ -458,6 +466,8 @@ class ObservationCapture: "image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key, "zero_joints": list(motion.reference_joints), "sample_phase": "zero_approach", "reference_only": motion.reference_only, + **({"preparation_witness_joint": motion.preparation_witness_joint, "cycle": motion.cycle} + if motion.preparation_witness_joint else {}), "command_unit": self.profile.command.unit, "command_vector": None if frame.command is None else list(frame.command), "feedback_vector": None if frame.feedback is None else list(frame.feedback), diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py index ac46d2a..c2d02d8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py @@ -15,6 +15,8 @@ class CaptureRecordIndex(Sequence): self.retain_training_geometry = retain_training_geometry self._rows = [] self._attempts = set() + self._by_task = {} + self._training_snapshots = {} def __len__(self): return len(self._rows) @@ -25,15 +27,34 @@ class CaptureRecordIndex(Sequence): def reset(self, rows=()): self._rows.clear() self._attempts.clear() + self._by_task.clear() + self._training_snapshots.clear() self.extend(rows) def extend(self, rows): for row in rows: self.append(row) + def task_records(self, task_key): + return tuple(self._by_task.get(task_key, ())) + + def training_task_snapshot(self, task_key, cycles): + rows = self._by_task.get(task_key, ()) + key = tuple(cycles), len(rows) + cached = self._training_snapshots.get(task_key) + if cached is None or cached[0] != key: + cached = key, task_training_snapshot(rows, cycles) + self._training_snapshots[task_key] = cached + return cached[1] + + def _store(self, row): + self._rows.append(row) + if row.get("task_name") is not None: + self._by_task.setdefault(row["task_name"], []).append(row) + def append(self, row): if self.retain_complete: - self._rows.append(row) + self._store(row) return kind = row.get('kind') if kind in {'joint_sample', 'secondary_joint_sample'}: @@ -42,17 +63,19 @@ class CaptureRecordIndex(Sequence): # live training-grid selector. Pose provenance stays in the journal. indexed = {key: value for key, value in row.items() if value is None or isinstance(value, (str, int, float, bool))} - for key in ('relative_quaternion_xyzw', 'steady_training_nodes'): + for key in ('relative_quaternion_xyzw', 'steady_training_nodes', 'command_direction_by_index'): if key in row: indexed[key] = tuple(row[key]) if self.retain_training_geometry: from ..core.fitting.training_quality import TRAINING_FIELDS + from ..core.domain.capture_plan import evidence_digest indexed.update({key: row[key] for key in TRAINING_FIELDS if key in row}) - self._rows.append(indexed) - elif kind in {'command_sampling_plan', 'training_decision'}: - self._rows.append(row) + indexed['source_record_sha256'] = evidence_digest(row) + self._store(indexed) + elif kind in {'command_sampling_plan', 'training_decision', 'staged_training_decision'}: + self._store(row) elif self.retain_training_geometry and kind in {'session_start', 'joint_zero_reference', 'scan_unit_complete'}: - self._rows.append(row) + self._store(row) elif all(key in row for key in ('task_name', 'cycle', 'direction')): # An unsuccessful later attempt can contain images but no valid # joint sample. Preserve its identity so it cannot expose old data. @@ -60,4 +83,26 @@ class CaptureRecordIndex(Sequence): identity = tuple(row.get(key) for key in keys) if identity not in self._attempts: self._attempts.add(identity) - self._rows.append({key: row[key] for key in keys if key in row}) + self._store({key: row[key] for key in keys if key in row}) + + +def task_training_snapshot(records, cycles): + from ..core.domain.capture_plan import evidence_digest + from ..core.domain.motion_path import record_scan_identity + from ..core.fitting.training_quality import TRAINING_FIELDS + latest = {} + for row in records: + if row.get('cycle') in cycles: + key = record_scan_identity(row) + latest[key] = max(latest.get(key, 0), int(row.get('attempt', 1))) + selected = [] + for row in records: + if (row.get('kind') != 'joint_sample' or row.get('cycle') not in cycles + or row.get('sample_phase', 'sweep') not in {'sweep', 'steady'} + or row.get('attempt', 1) != latest[record_scan_identity(row)]): + continue + snapshot = {key: row[key] for key in TRAINING_FIELDS if key in row} + snapshot['command_direction_by_index'] = row.get('command_direction_by_index', []) + snapshot['source_record_sha256'] = row.get('source_record_sha256') or evidence_digest(row) + selected.append(snapshot) + return tuple(selected) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index dc6a381..c66ee45 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -42,7 +42,7 @@ from .resume import ResumeDecision, ResumeVerifier, fingerprint_from_mapping from .safety import SafetyPolicy, SafetySample from .session import CalibrationPhase as Phase from .snapshot import DeviceReadiness, CameraReadiness, RuntimeSnapshot, build_snapshot, legacy_state -from ..core.domain.capture_plan import CapturePlan, ADAPTIVE +from ..core.domain.capture_plan import CapturePlan, ADAPTIVE, STAGED _TERMINAL_STATES = frozenset({"PASSED", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"}) @@ -52,6 +52,8 @@ class CalibrationCoordinator: def __init__(self, profile: CalibrationProfile, parameters: RuntimeParameters, ports: RuntimePorts, adapter_factory: AdapterFactory, *, finalization=None, initialization_solver=None, projection_worker=None): + if profile.quality.training_policy == STAGED: + parameters = replace(parameters, resume_mode="verify") self.profile, self.parameters, self.ports = profile, parameters, ports from .timing_diagnostics import StageTiming self.stage_timing = StageTiming(ports.monotonic()) @@ -72,7 +74,8 @@ class CalibrationCoordinator: self.execution = SessionExecution(profile, diagnostic_capture=self.diagnostic_capture) self.training_worker = None self._training_request = None - if profile.quality.training_policy == ADAPTIVE: + self._frozen_training_sha256 = None + if profile.quality.training_policy in {ADAPTIVE, STAGED}: if self.diagnostic_capture is not None: raise ValueError("adaptive_training_cannot_run_as_diagnostic") from .training import TrainingWorker @@ -93,7 +96,7 @@ class CalibrationCoordinator: from .capture_index import CaptureRecordIndex self.capture_index = CaptureRecordIndex(retain_complete=( self.diagnostic_capture is not None or not getattr(self.finalization, "uses_journal", False)), - retain_training_geometry=profile.quality.training_policy == ADAPTIVE) + retain_training_geometry=profile.quality.training_policy in {ADAPTIVE, STAGED}) self._image_record_stamps = {} self.last_command = None self.commanded_speed = None @@ -133,7 +136,8 @@ class CalibrationCoordinator: try: self._resume_checkpoint = PreparedResume.load(profile, self.execution.session.engine, parameters.resume_raw_samples_path, parameters.serial_number, - image_replay_workers=checkpoint_replay_workers(), resume_mode=parameters.resume_mode) + image_replay_workers=checkpoint_replay_workers(), resume_mode=parameters.resume_mode, + source_urdf=parameters.source_urdf) except (OSError, ValueError, KeyError, TypeError) as error: self._resume_checkpoint_error = str(error) self.stage_timing.enter("startup", ports.monotonic()) @@ -239,8 +243,10 @@ class CalibrationCoordinator: self._branch_observation_reasons.clear() self.branch_initialization.clear() self.branch_initialization.parent_references.clear() + self.branch_initialization.witness_capture.clear() self.execution.session.start(resume_requested=self.parameters.resume_raw_samples_path is not None) self._started_at = self.ports.monotonic() + self.stage_timing.mark_start(self._started_at) self._observation_epoch += 1 self.reason = "moving_to_safe_baseline" self._publish_torque() @@ -457,6 +463,7 @@ class CalibrationCoordinator: self._image_record_stamps[view] = stamp row.update(session_epoch=epoch, motion_version=motion_version, motion_started_ns=self._motion_started_ns) + self.branch_initialization.parent_references.handoff.observe(row) if row.get("kind") == "diagnostic_observation_frame": row.update(session_epoch=epoch, motion_version=motion_version, motion_started_ns=self._motion_started_ns) @@ -500,7 +507,7 @@ class CalibrationCoordinator: if self.diagnostic_capture is None: return None if motion is not None and motion.phase == "joint_zero": - ready = motion.reference_reuse or self.branch_initialization.ready(motion) + ready = motion.reference_reuse or motion.reference_check or self.branch_initialization.ready(motion) return frozenset(motion.zero_joints) if ready else frozenset() return frozenset(self.execution.zero_references) @@ -625,10 +632,19 @@ class CalibrationCoordinator: from .passed_resume import restore_parent_models, restored_model_record binding = restored_model_record(pending.model_records, self._observation_epoch, self.ports.clock_ns()) + if self.profile.quality.training_policy == STAGED: + binding.update(kind="staged_pose_models_restored", resume_mode="verify", + physical_zero_reverification_required=True) + binding.pop("unchanged_installation_declared", None) append_jsonl(self.raw_path, binding) self.capture_index.append(binding) restore_parent_models(self.branch_initialization.parent_references, pending.model_records, self._observation_epoch) + from .observer_reference import restore_observer_records + restore_observer_records(self.branch_initialization, pending.rows) + for row in pending.rows: + if row.get("kind") == "motion_branch_observation": + self.branch_initialization.witness_capture.observe(row) self.execution.zero_references.update(pending.references) if self.visual_motion is not None: self.visual_motion.freeze_zeros(pending.references) @@ -643,7 +659,11 @@ class CalibrationCoordinator: self._import_resume_batch() return if phase in {Phase.PREPARE, Phase.MAPPING_PROBE} and self.segment is None and session.current_unit is not None: - used, imported = self.joint_resume.take_task(session.current_unit.task_key) + unit = session.current_unit + if self.profile.quality.training_policy == STAGED: + used, imported = self.joint_resume.take_task(unit.task_key, cycle=unit.cycle) + else: + used, imported = self.joint_resume.take_task(unit.task_key) if used: from .joint_resume import TaskResumeImport self._resume_import = TaskResumeImport(rows=imported, @@ -678,10 +698,14 @@ class CalibrationCoordinator: if self._training_request is None: from ..core.fitting.training_quality import training_snapshot cycles = tuple(range(action.scan_unit.cycle + 1)) + if self.profile.quality.training_policy == STAGED: + from .staged_training import staged_snapshot + rows = staged_snapshot(self.execution.profile, self.capture_index) + else: + rows = training_snapshot(self.execution.profile, self.capture_index, action.scan_unit.task_key, cycles) self._training_request = dict(profile=self.execution.profile, task_key=action.scan_unit.task_key, cycles=cycles, - rows=training_snapshot(self.execution.profile, self.capture_index, - action.scan_unit.task_key, cycles), + rows=rows, references=dict(self.execution.zero_references), source_urdf=self.parameters.source_urdf) try: training_decision = self.training_worker.poll(**self._training_request) @@ -692,6 +716,12 @@ class CalibrationCoordinator: self.reason = "training_quality_assessment:" + action.scan_unit.task_key self._publish_command(list(current)) return + if self.profile.quality.training_policy == STAGED: + payload = training_decision["frozen_training"] + training_decision = training_decision["decision"] + if payload is not None: + atomic_write_json(self.parameters.session_dir / "frozen_training.json", payload) + self._frozen_training_sha256 = training_decision["frozen_training_sha256"] self._training_request = None quality = self.execution.evaluate(self.capture_index, training_decision=training_decision) record = {"kind": "scan_unit_complete", "task_name": action.scan_unit.task_key, @@ -1006,7 +1036,7 @@ class CalibrationCoordinator: self.visual_motion.freeze_zeros(pending.references) self._reference_import = None return True - if not self.branch_initialization.ready(motion): + if not motion.reference_check and not self.branch_initialization.ready(motion): self._zero_reference_error = (self.branch_initialization.error() or "motion_geometry_confirmation_pending") return False @@ -1031,6 +1061,21 @@ class CalibrationCoordinator: except ValueError as error: self._zero_reference_error = str(error) return False + if motion.reference_check: + from .revisit import revisit_record + try: + checks = [revisit_record(self.profile, self.execution.zero_references[name], reference, motion.cycle) + for name, reference in references.items()] + except (ValueError, KeyError) as error: + self._pause(str(error)) + return False + if not self._freeze_reference_branches(revisions): + self._zero_reference_error = "joint_zero_motion_branch_not_confirmed" + return False + append_jsonl_many(self.raw_path, checks) + self.capture_index.extend(checks) + self.joint_resume.verified_visits.update((name, motion.cycle) for name in references) + return True if references.keys() & self.execution.zero_references.keys(): raise RuntimeError("a frozen joint reference cannot be replaced") current_references = dict(references) @@ -1102,9 +1147,11 @@ class CalibrationCoordinator: referenced_joints=() if motion.reference_only else self.execution.zero_references) if action is None: return False - from .zero_recovery import failure_category + from .zero_recovery import FailureCategory record = {"kind": "joint_zero_recovery", "task_name": motion.task_key, - "failure_category": failure_category(geometry_error or self._zero_reference_error).value, + # Admission above allows only missing evidence, even when the + # human-readable message wraps a structured per-view reason. + "failure_category": FailureCategory.MISSING_SAMPLES.value, "capture_plan_sha256": CapturePlan.from_profile(self.execution.profile).sha256, "joints": list(motion.zero_joints), "action": action, "reason": geometry_error or self._zero_reference_error, "previous_session_epoch": self._observation_epoch, "session_epoch": self._observation_epoch+1, @@ -1163,6 +1210,14 @@ class CalibrationCoordinator: self._steady_rows = [] self._zero_reference_error = "" self.capture.begin_motion(motion) + if motion.reference_check and motion.phase == "joint_zero": + try: + self.branch_initialization.parent_references.activate_joints( + motion.zero_joints, self._observation_epoch, self.capture) + self._zero_geometry_completed_at = self.ports.monotonic() + except ValueError as error: + self._pause(str(error)) + return if motion.reference_reuse: task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) try: @@ -1450,10 +1505,24 @@ class CalibrationCoordinator: # batches the ordinary feedback, abort and safety callbacks run. self.execution.first_cycle_spans.update(self.joint_resume.spans_for_task(pending.task_key)) cycles = self.joint_resume.profile.quality.task_training_cycles.get(pending.task_key) - if cycles is not None: + staged = self.profile.quality.training_policy == STAGED + if cycles is not None and not staged: self.execution.session.select_training(pending.task_key, cycles) self.execution.profile = self.execution.session.profile + decisions = [r for r in pending.rows if r.get("kind") == "staged_training_decision"] if staged else [] + for record in decisions: + if record["decision"] == "supplement": + self.execution.session.supplement_training(record["supplement_tasks"]) + self.execution.profile = self.execution.session.profile + elif record["decision"] == "freeze": + self._frozen_training_sha256 = record["frozen_training_sha256"] self.execution.authorize_task_reuse(pending.units) + if staged and not decisions: + last = max((i for i, u in enumerate(self.execution.session._units) if u.identity in pending.units)) + unit = self.execution.session._units[last] + if unit.cycle in {1, 2} and unit == [u for u in self.execution.session._units if u.cycle == unit.cycle][-1]: + self.execution.session._unit_index = last + self.execution.session.phase = Phase.EVALUATE self._resumed_count += len(pending.units) self._resume_import = None @@ -1489,10 +1558,19 @@ class CalibrationCoordinator: if self.profile.artifacts.output_schema_version >= 3: self.joint_resume = checkpoint.joints decision = replace(decision, reason="整场参考通过，等待逐关节验证原零位后复用") + if self.profile.quality.training_policy == STAGED: + from .staged_resume import restore_recovery + self._passed_reference_import = checkpoint.passed_import + restore_recovery(self.execution.session, self.zero_recovery, rows) + if checkpoint.frozen_training is not None: + atomic_write_json(self.parameters.session_dir / "frozen_training.json", checkpoint.frozen_training) else: decision = replace(decision, completed_units=tuple(sorted(complete))) except (OSError, ValueError, KeyError, TypeError) as error: decision = ResumeDecision(False, "旧断点校验未通过，已重新采集", (str(error),)) + if self.profile.quality.training_policy == STAGED and not decision.reuse: + self._pause("staged_resume_reference_or_evidence_changed:" + str(decision.incompatible_fields)) + return decision = self.execution.restore(decision, rows) if decision.reuse: used = set(decision.completed_units) @@ -1549,6 +1627,7 @@ class CalibrationCoordinator: if self.capture_index.retain_complete else ()), require_motion_evidence=True, journal_path=self.raw_path, journal_size=self.raw_path.stat().st_size, camera_extrinsics_file=self.parameters.camera_extrinsics_file, + frozen_training_sha256=self._frozen_training_sha256, )) def _poll_finalization(self) -> None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py index 6506998..d7892a1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -15,7 +15,7 @@ from .diagnostic_capture import DiagnosticCapturePlan from ..core.domain.profile import ACQUISITION_POLICY_VERSION from ..core.domain.motion_path import task_segments, scan_identity -from ..core.domain.capture_plan import CapturePlan, ADAPTIVE, LEGACY_TRAINING, LEGACY_HOLDOUT +from ..core.domain.capture_plan import CapturePlan, ADAPTIVE, STAGED, LEGACY_TRAINING, LEGACY_HOLDOUT TRAINING_CYCLES = LEGACY_TRAINING HOLDOUT_CYCLE = LEGACY_HOLDOUT CAPTURE_SCHEDULE_VERSION = "unified_schedule_v7_single_pass_endpoint_images" @@ -26,8 +26,9 @@ VISUAL_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v9_visual_motion" PARENT_REFERENCE_SCHEDULE_VERSION = "unified_schedule_v10_verified_parent_geometry" IMAGE_REFERENCE_SCHEDULE_VERSION = "unified_schedule_v11_stationary_image_reference" ADAPTIVE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v12_adaptive_training" +STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v13_staged_training" SUPPORTED_CAPTURE_SCHEDULE_VERSIONS = frozenset((CAPTURE_SCHEDULE_VERSION, - ADAPTIVE_CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION, *(f"{SEGMENTED_CAPTURE_SCHEDULE_VERSION}_{mode}" + STAGED_CAPTURE_SCHEDULE_VERSION, ADAPTIVE_CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION, *(f"{SEGMENTED_CAPTURE_SCHEDULE_VERSION}_{mode}" for mode in ("interleaved", "separate")), *(f"{PATH_CAPTURE_SCHEDULE_VERSION}_{mode}" for mode in ("interleaved", "separate")), *(f"{VISUAL_CAPTURE_SCHEDULE_VERSION}_{mode}" for mode in ("interleaved", "separate")), *(f"{PARENT_REFERENCE_SCHEDULE_VERSION}_{feedback}_{mode}" @@ -37,6 +38,8 @@ SUPPORTED_CAPTURE_SCHEDULE_VERSIONS = frozenset((CAPTURE_SCHEDULE_VERSION, def capture_schedule_version(profile): + if profile.quality.training_policy == STAGED: + return STAGED_CAPTURE_SCHEDULE_VERSION if profile.quality.training_policy == ADAPTIVE: return ADAPTIVE_CAPTURE_SCHEDULE_VERSION if profile.acquisition.fixed_reference_mode == "stationary_image": @@ -197,6 +200,8 @@ class CalibrationEngine: for cycle in (*command_training_cycles(self.profile, task), command_holdout_cycle(self.profile)): units.extend(ScanUnit(task.key, cycle, segment.direction, segment.start, segment.end, speed, "steady", segment.key) for segment in task_segments(task)) + if self.profile.quality.training_policy == STAGED: + units.sort(key=lambda unit: unit.cycle) # Stable: declared task/segment order within a pass. return tuple(units) def mapping_probe_delta(self, task: TaskSpec) -> float | None: @@ -282,7 +287,7 @@ class CalibrationEngine: and session_start.get("acquisition_policy_version") == ACQUISITION_POLICY_VERSION and session_start.get("capture_schedule_version") in ( - (capture_schedule_version(self.profile),) if (self.profile.quality.training_policy == ADAPTIVE or self.profile.acquisition.fixed_reference_mode == "stationary_image" + (capture_schedule_version(self.profile),) if (self.profile.quality.training_policy in {ADAPTIVE, STAGED} or self.profile.acquisition.fixed_reference_mode == "stationary_image" or self.profile.vision_motion or any(task.segments for task in self.profile.motion.tasks) or any(task.parent_reference is not None for task in self.profile.motion.tasks) or any(spec.evidence_scope == "path" for spec in self.profile.motion.joint_zero_references.values()) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py index 84d6f73..25987b5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py @@ -35,6 +35,7 @@ class SessionExecution: self._effects = [] self._action_key = None self._entered_task = None + self._entered_visit = None self.zero_references = {} self.sampling_plans = {} self.diagnostic_zero_attempts: dict[str, DiagnosticZeroAttempt] = {} @@ -54,13 +55,18 @@ class SessionExecution: def _make_effects(self, current): action = self.action + from ..core.domain.capture_plan import STAGED + staged = self.profile.quality.training_policy == STAGED + visit = None if action.scan_unit is None else (action.scan_unit.task_key, action.scan_unit.cycle) + new_visit = staged and visit != self._entered_visit + previous_visit = self._entered_visit params = self.profile.motion.speed_parameters baseline_speed = float(params.get("baseline_rad_s", 0.1) if self.profile.command.unit == "rad" else params.get("baseline_u8", params.get("preflight_u8", 1))) exits = () if action.phase in {Phase.PREPARE, Phase.RETURN_BASELINE}: next_task = None if action.scan_unit is None else action.scan_unit.task_key - if self._entered_task is not None and self._entered_task != next_task: + if self._entered_task is not None and (self._entered_task != next_task or new_visit): previous = next(t for t in self.profile.motion.tasks if t.key == self._entered_task) pose, effects = list(current), [] for waypoint in previous.exit_waypoints: @@ -100,19 +106,44 @@ class SessionExecution: measured_channels=segment.moving_channels) if action.phase == Phase.PREPARE: entry = [] - if self._entered_task != task.key: + if new_visit and previous_visit is not None and (previous_visit[1] != unit.cycle or unit.cycle == 2): + targets = build_calibration_return_waypoints(profile=self.profile, current_command=current) + for target in targets: + entry.append(MotionCommand("stage_return", target, baseline_speed, **common)) + current = target + entering = self._entered_task != task.key or new_visit + if entering: pose = list(current) for waypoint in task.entry_waypoints: for index, value in waypoint: pose[index] = value entry.append(MotionCommand("clearance", tuple(pose), unit.speed, **common)) self._entered_task = task.key + self._entered_visit = visit current = tuple(pose) if self.profile.artifacts.output_schema_version >= 3: pending = {name: spec for name, spec in self.profile.motion.joint_zero_references.items() - if spec.task_key == task.key and name not in self.zero_references + if spec.task_key == task.key and (name not in self.zero_references or staged and entering) and not (self.diagnostic_capture is not None and name in self.diagnostic_zero_attempts)} - if entry or not any(name in self.zero_references for name in task.joints): + if entering or not any(name in self.zero_references for name in task.joints): + for name in task.joints: + witness = self.profile.motion.preparation_witnesses.get(name) + if witness is None or name in self.zero_references or unit.cycle == self.profile.quality.holdout_cycle: + continue + # Reach the existing task-specific avoidance pose first. + # Excitation then changes exactly one upstream channel; + # it is not a new curve/zero or a formal scan repetition. + entry.extend(MotionCommand("clearance", point, unit.speed, **common) + for point in build_joint_reference_waypoints(task, witness.command, + profile=self.profile, current_command=current)) + current = witness.command + from ..core.fitting.motion_fit import channel_for_joint + metadata = dict(common, command_index=channel_for_joint(self.profile, witness.joint), + reference_joints=(name,), preparation_witness_joint=witness.joint) + for path_index, target in enumerate((*witness.approach_commands, witness.command)): + entry.append(MotionCommand("zero_approach", target, unit.speed, + reference_path_index=path_index, **metadata)) + current = target if task.parent_reference is not None: name = task.parent_reference.pose_joint target = self.profile.motion.joint_zero_references[task.joints[0]].command @@ -122,7 +153,8 @@ class SessionExecution: entry.append(MotionCommand("joint_zero", target, unit.speed, zero_joints=(name,), reference_only=True, reference_reuse=True, **common)) current = target - for preparation in task.model_preparations: + preparations = () if staged and all(name in self.zero_references for name in task.joints) else task.model_preparations + for preparation in preparations: metadata = dict(common, reference_only=True, reference_command=preparation.command, reference_approach=preparation.approach_commands) @@ -139,15 +171,17 @@ class SessionExecution: groups.setdefault((spec.command, spec.approach_commands, spec.evidence_scope, name if task.segments else ""), []).append(name) for (target, approach, scope, _), names in groups.items(): + checking = all(name in self.zero_references for name in names) for path_index, pose in enumerate((*approach, target)): entry.extend(MotionCommand("zero_approach", point, unit.speed, reference_joints=tuple(sorted(names)), + reference_check=checking, reference_path_index=path_index if scope == "path" else None, **common) for point in build_joint_reference_waypoints(task, pose, profile=self.profile, current_command=current)) current = pose entry.append(MotionCommand("joint_zero", target, unit.speed, - zero_joints=tuple(sorted(names)), **common)) + zero_joints=tuple(sorted(names)), reference_check=checking, **common)) current = target return exits + tuple(entry) + tuple(MotionCommand("prepare", target, unit.speed, **common) for target in build_calibration_preparation_waypoints(task, profile=self.profile, @@ -272,14 +306,15 @@ class SessionExecution: self.diagnostic_zero_attempts.update({name: attempt for name in attempt.joint_names}) def training_check_due(self): - from ..core.domain.capture_plan import ADAPTIVE + from ..core.domain.capture_plan import ADAPTIVE, STAGED unit = self.action.scan_unit - if (self.profile.quality.training_policy != ADAPTIVE or self.diagnostic_capture is not None + if (self.profile.quality.training_policy not in {ADAPTIVE, STAGED} or self.diagnostic_capture is not None or unit is None or unit.cycle not in (1, 2) or (unit.task_key, unit.cycle) in self._training_checks): return False round_units = [u for u in self.session.engine.scan_units() - if u.task_key == unit.task_key and u.cycle == unit.cycle] + if (self.profile.quality.training_policy == STAGED or u.task_key == unit.task_key) + and u.cycle == unit.cycle] return unit.identity == round_units[-1].identity def evaluate(self, records, *, training_decision=None): @@ -303,10 +338,11 @@ class SessionExecution: if self.training_check_due(): if training_decision is None: raise ValueError("adaptive_training_requires_assessment_before_advancing") - from ..core.domain.capture_plan import CapturePlan, evidence_digest - if (training_decision.get("task_name") != unit.task_key + from ..core.domain.capture_plan import CapturePlan, STAGED, evidence_digest + staged = self.profile.quality.training_policy == STAGED + if ((not staged and training_decision.get("task_name") != unit.task_key) or training_decision.get("input_plan_sha256") != CapturePlan.from_profile(self.profile).sha256 - or training_decision.get("assessed_cycles") != list(range(unit.cycle + 1)) + or (not staged and training_decision.get("assessed_cycles") != list(range(unit.cycle + 1))) or training_decision.get("decision_sha256") != evidence_digest({k: v for k, v in training_decision.items() if k != "decision_sha256"})): raise ValueError("training_assessment_does_not_match_current_round") @@ -314,7 +350,11 @@ class SessionExecution: self._training_checks.add((unit.task_key, unit.cycle)) decision = training_decision["decision"] if decision == "freeze": - self.session.select_training(unit.task_key, training_decision["assessed_cycles"]) + if not staged: + self.session.select_training(unit.task_key, training_decision["assessed_cycles"]) + self.profile = self.session.profile + elif staged and decision == "supplement" and unit.cycle == 1: + self.session.supplement_training(training_decision["supplement_tasks"]) self.profile = self.session.profile elif decision == "fail": self.session.fail("training_quality_failed", ",".join(training_decision["failures"])) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py index 9144dad..b81e17d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py @@ -60,9 +60,10 @@ class PreparedResume: completed_units: frozenset joints: JointResume passed_import: ResumeJournalImport | None = None + frozen_training: dict | None = None @classmethod - def load(cls, profile, engine, path, serial_number, *, image_replay_workers=1, resume_mode="verify"): + def load(cls, profile, engine, path, serial_number, *, image_replay_workers=1, resume_mode="verify", source_urdf=None): from .acquisition import load_capture from .resume_storage import load_passed_capture from .diagnostic_capture import reject_diagnostic_capture @@ -80,11 +81,22 @@ class PreparedResume: raise ValueError("checkpoint identity/policy changed") from .training import resolve_capture_plan from .engine import CalibrationEngine - profile = resolve_capture_plan(profile, rows) + from ..core.domain.capture_plan import STAGED + profile = resolve_capture_plan(profile, rows, source_urdf=source_urdf, + verify_decisions=profile.quality.training_policy == STAGED and source_urdf is not None) engine = CalibrationEngine(profile) complete = frozenset(record_scan_identity(r) for r in rows if r.get("kind") == "scan_unit_complete" and r.get("passed") is True) joints = JointResume(profile, image_replay_workers=image_replay_workers) + if profile.quality.training_policy == STAGED: + from .staged_resume import stage_visits, load_frozen_checkpoint, reference_import + stage_visits(joints, engine, rows) + frozen = load_frozen_checkpoint(profile, rows, path, source_urdf) + # Index/hash historical provenance before live callbacks start. + # The control loop only authorizes and drains this bounded import. + pending = reference_import(joints) + return cls(header, references[0], rows, frozenset(joints.units), joints, + passed_import=pending, frozen_training=frozen) passed_import = None if profile.artifacts.output_schema_version >= 3: if resume_mode == "passed": @@ -110,14 +122,21 @@ def _index_motion_provenance(rows): zero_images = {(r.get("view"), r.get("image_stamp_ns")) for r in rows if r.get("kind") == "joint_zero_sample"} + handoff_images = {(r.get('view'), int(stamp)) for r in rows + if r.get('kind') == 'motion_branch_initialization' + for bridge in r.get('shared_tag_pose_bridges', ()) + for stamp in bridge.get('source_window_hashes', {})} kinds = {"motion_branch_observation", "motion_branch_initialization", "capture_scope_extension", "capture_observation_revision", "passed_tasks_reused", "passed_pose_models_restored", + "staged_pose_models_restored", "resume_verification", "parent_reference_verified", "joint_zero_recovery", "joint_zero_motion_reference_verified", "joint_zero_sample", "visual_motion_observation", "visual_motion_settled"} selected = (materialize_record(row) for row in rows - if row.get("kind") in kinds or (row.get("kind") == "pnp_candidate_frame" + if row.get("kind") in kinds or (row.get('kind') == 'image_observation_frame' + and (row.get('view'), row.get('image_stamp_ns')) in handoff_images) + or (row.get("kind") == "pnp_candidate_frame" and (row.get("view"), row.get("image_stamp_ns")) in zero_images)) return {source_frame_sha256(row): row for row in selected} @@ -146,6 +165,7 @@ class JointResume: self.image_replay_workers = image_replay_workers self.references = {} self.verified = set() + self.verified_visits = set() self.units = set() self.rows = () self.first_cycle_spans = {} @@ -275,7 +295,11 @@ class JointResume: self._motion_provenance_pending.clear() return tuple(rows) - def take_task(self, task_key): + def take_task(self, task_key, cycle=None): + from ..core.domain.capture_plan import STAGED + if self.profile.quality.training_policy == STAGED: + from .staged_resume import visit_rows + return visit_rows(self, task_key, cycle) task = next(t for t in self.profile.motion.tasks if t.key == task_key) if not set(task.joints) <= self.verified: return (), () diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index dfcd40a..5d9cc81 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -50,9 +50,11 @@ class MotionCommand: measured_channels: tuple[int, ...] = () reference_only: bool = False reference_reuse: bool = False + reference_check: bool = False reference_command: tuple[float, ...] = () reference_approach: tuple[tuple[float, ...], ...] = () reference_path_index: int | None = None + preparation_witness_joint: str = "" feedback_targets: tuple[tuple[int, float], ...] = () visual_return_joints: tuple[str, ...] = () defer_settling_to_hold: bool = False diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py index 3444210..0487035 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py @@ -97,16 +97,44 @@ def _initializations(records): _fail("source_image_changed") observations[key] = row if row.get("kind") == "motion_branch_initialization" and row.get("status") == "resolved": + if ("image_solver_policy" in row or "image_solver_attempts" in row + or any(len(h.get("training_solver_attempts", ())) > 1 for h in row.get("hypotheses", ()))): + from ..core.geometry.tag_pose.image_bundle_optimizer import IMAGE_BUNDLE_SOLVER_POLICY + expected_attempts = [{key: hypothesis.get(key) for key in ( + "branches", "training_solver_attempts", "training_solver_status", "training_solver_evaluations")} + for hypothesis in row.get("hypotheses", ())] + if (row.get("image_solver_policy") != IMAGE_BUNDLE_SOLVER_POLICY + or row.get("image_solver_attempts") != expected_attempts): + _fail("solver_report_changed") + from .preparation_transition_evidence import check_transition_report + check_transition_report(row) + from .preparation_witness_evidence import check_witness_report + check_witness_report(row) + from ..core.geometry.tag_pose.pose_bridge import OBSERVER_POSE_SELECTION_POLICIES + policy = row.get('image_model_selection_policy') + if (policy in OBSERVER_POSE_SELECTION_POLICIES) != bool(row.get('observer_pose_bridges')): + _fail('observer_pose_selection_policy_changed') _check_measured_transfer_report(row) - if ((row.get('image_model_selection_policy') in SHARED_TAG_CONDITIONING_POLICIES) + shared_policy = OBSERVER_POSE_SELECTION_POLICIES.get(policy, policy) + from ..core.geometry.tag_pose.pose_bridge import SHARED_TAG_HANDOFF_POLICY + from .shared_pose_handoff import HANDOFF_POLICY + if any((bridge.get('handoff_policy') == HANDOFF_POLICY) + != (shared_policy == SHARED_TAG_HANDOFF_POLICY) + for bridge in row.get('shared_tag_pose_bridges', ())): + _fail('shared_pose_handoff_policy_changed') + if ((shared_policy in SHARED_TAG_CONDITIONING_POLICIES) != bool(row.get('shared_tag_pose_bridges'))): _fail('shared_pose_selection_policy_changed') - conditioning = SHARED_TAG_CONDITIONING_POLICIES.get(row.get('image_model_selection_policy')) + conditioning = SHARED_TAG_CONDITIONING_POLICIES.get(shared_policy) if any(bridge.get('conditioning') != conditioning for bridge in row.get('shared_tag_pose_bridges', ())): _fail('shared_pose_conditioning_policy_changed') resolved.append(row) for role, payload in row.get("evidence_payloads", {}).items(): + if (shared_policy == SHARED_TAG_HANDOFF_POLICY + or 'image_model_selection_policy' in payload): + if payload.get('image_model_selection_policy') != policy: + _fail('shared_pose_handoff_policy_changed') identity = row.get("evidence_ids", {}).get(role) if (not isinstance(identity, str) or re.fullmatch(r"[0-9a-f]{64}", identity) is None or hashlib.sha256(_canonical(payload).encode()).hexdigest() != identity): @@ -115,8 +143,10 @@ def _initializations(records): "task_name", "view", "zero_joints", "session_epoch", "motion_version", "source_image_stamps", "source_frame_hashes", "constraints", "frozen_image_model", "image_model_sha256", + "image_solver_policy", "image_solver_attempts", "image_sampling_policy", "geometry_uncertainty", "parent_reference_sha256", - "approach_evidence_scope", "source_motion_versions", "shared_tag_pose_bridges")): + "approach_evidence_scope", "source_motion_versions", "shared_tag_pose_bridges", "observer_pose_bridges", + "preparation_transitions", "preparation_witnesses")): _fail("initialization_binding_changed") stamps = payload.get("source_image_stamps", ()) if (not stamps or any(type(stamp) is not int or stamp < 1 for stamp in stamps) @@ -147,6 +177,12 @@ def _initializations(records): _fail("image_model_source_urdf_changed") from .parent_reference_evidence import validate_source_hinges validate_source_hinges(evidence, records) + if any(payload.get('preparation_transitions') for payload in evidence.values()): + from .preparation_transition_evidence import transition_inputs + tuple(transition_inputs(records, evidence)) + if any(payload.get('preparation_witnesses') for payload in evidence.values()): + from .preparation_witness_evidence import witness_inputs + tuple(witness_inputs(records, evidence)) return evidence @@ -197,6 +233,21 @@ def validate_source_geometry(profile, source_urdf, records): or model.constraints != compile_parallel_axis_geometry(profile, source_model, tuple(item.geometry.relation for item in getattr(model, "source_hinges", ())) + model.relations)): _fail("image_model_source_geometry_changed") + if any(row.get('observer_pose_bridges') for row in records): + from .observer_reference import validate_observer_channels + if source_model is None: + source_model = UrdfKinematicModel(source_urdf) + validate_observer_channels(profile, records, source_model) + if profile.motion.preparation_witnesses or any(row.get('preparation_witnesses') for row in records): + from .preparation_witness_evidence import validate_witness_topology + if source_model is None: + source_model = UrdfKinematicModel(source_urdf) + validate_witness_topology(profile, records, _initializations(records), source_model) + if any(row.get('preparation_transitions') for row in records): + from .preparation_transition_evidence import validate_transition_topology + if source_model is None: + source_model = UrdfKinematicModel(source_urdf) + validate_transition_topology(profile, records, _initializations(records), source_model) if source_model is not None and any(task.parent_reference is not None for task in profile.motion.tasks): from .parent_reference_evidence import validate_parent_references validate_parent_references(profile, records, _initializations(records), source_model=source_model) @@ -209,9 +260,13 @@ def validate_source_geometry(profile, source_urdf, records): def _check_image_initialization(payload, observations): model = frozen_image_model(payload) + from ..core.geometry.tag_pose.production_image_motion import image_parameters_for_sampling + limits = image_parameters_for_sampling(payload.get("image_sampling_policy")) if (list(model.source_stamps_ns) != payload["source_image_stamps"] or len(model.source_stamps_ns) < 24 or min(len(model.training_stamps_ns), len(model.validation_stamps_ns)) < 12 + or len(model.training_stamps_ns) > limits.maximum_training_frames + or len(model.validation_stamps_ns) > limits.maximum_validation_frames or payload["tag_role"] not in {item.relation.child_role for item in model.geometry} or not {item.relation.joint for item in model.geometry} <= set(payload["zero_joints"])): _fail("image_model_support_or_roles_changed") @@ -277,7 +332,7 @@ def _reference_authorizations(profile, reference, evidence): first_zero = min(sample.image_stamp_ns for sample in reference.samples if sample.view == item.view) if max(payload["source_image_stamps"]) >= first_zero: _fail(f"initialization_not_before_zero:{reference.joint}") - result[(item.view, item.tag_role, item.motion_evidence_id)] = (item.branch_revision, first_zero) + result[(item.view, item.tag_role, item.motion_evidence_id, item.branch_revision)] = first_zero return result @@ -297,12 +352,21 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp if not references or (require_complete and not expected <= references.keys()): _fail("joint_zero_references_missing") evidence = _initializations(records) + if any(payload.get('preparation_transitions') for payload in evidence.values()): + from .preparation_transition_evidence import validate_preparation_transitions + validate_preparation_transitions(profile, records, evidence) + if profile.motion.preparation_witnesses or any(payload.get('preparation_witnesses') for payload in evidence.values()): + from .preparation_witness_evidence import validate_preparation_witnesses + validate_preparation_witnesses(profile, records, evidence) _check_reference_modes(profile, records, evidence) from .parent_reference_evidence import validate_parent_references validate_parent_references(profile, records, evidence) from .shared_tag_evidence import validate_shared_tag_references if any(payload.get('shared_tag_pose_bridges') for payload in evidence.values()): validate_shared_tag_references(profile, records, evidence) + if any(payload.get('observer_pose_bridges') for payload in evidence.values()): + from .observer_reference import validate_observer_references + validate_observer_references(profile, records, evidence) image_frames, image_samples = _image_frames(records), {} authorizations = {name: _reference_authorizations(profile, reference, evidence) for name, reference in references.items()} @@ -320,7 +384,12 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp if (_canonical(original[name].as_record()) != _canonical(references[name].as_record()) or reference_pose_failures(original[name], current[name])): _fail(f"resume_reference_changed:{name}") - authorizations[name].update(_reference_authorizations(profile, current[name], evidence)) + if str(row.get("verification", "")).startswith("staged_revisit"): + from .revisit import verify_revisit + verify_revisit(profile, original[name], current[name], verification=row["verification"], + feedback_limits=row.get("feedback_limits")) + for key, stamp in _reference_authorizations(profile, current[name], evidence).items(): + authorizations[name][key] = min(authorizations[name].get(key, stamp), stamp) zero_images.update((name, sample.view, sample.image_stamp_ns) for sample in current[name].samples) cycles = {} for row in records: @@ -342,14 +411,13 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp for role in roles: item = by_role[role] diagnostics = item.get("candidate_diagnostics", {}) - key = (view, role, diagnostics.get("motion_evidence_id")) + key = (view, role, diagnostics.get("motion_evidence_id"), diagnostics.get("branch_revision")) authorization = authorizations[name].get(key) if (authorization is None or item.get("pose_source") not in {"current_image", IMAGE_POSE_SOURCE} or diagnostics.get("motion_evidence_verified") is not True - or diagnostics.get("branch_revision") != authorization[0] or diagnostics.get("observation_stamp_ns") != stamp or diagnostics.get("branch_status") != "tracking" - or (not is_zero and (diagnostics.get("branch_frozen") is not True or stamp <= authorization[1]))): + or (not is_zero and (diagnostics.get("branch_frozen") is not True or stamp <= authorization))): _fail(f"sample_not_authorized:{name}:{view}:{role}") payload = evidence[diagnostics["motion_evidence_id"]] if has_image_model(payload) or has_image_model(item): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/observer_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/observer_reference.py new file mode 100644 index 0000000..ef142cf --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/observer_reference.py @@ -0,0 +1,220 @@ +"""Optional descendant pixels from existing preparation movements only.""" + +from dataclasses import dataclass, asdict + +import numpy as np + +from ..core.geometry.tag_pose.motion_evidence import MotionRelation +from ..core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge +from ..core.geometry.tag_pose.observer_pose import fit_observer_pose +from ..core.geometry.tag_pose.image_motion_projection import select_image_motion_frame +from .branch_initialization import _pose +from .shared_tag_reference import bridge_frames +from .motion_execution import feedback_tolerances +from .motion_provenance import source_frame_sha256 + + +def observer_roles(profile, motion): + if motion.phase != 'zero_approach' or motion.reference_check or profile.vision_motion: + return set() + parents = {profile.measurement.measurements[n].child_role for n in motion.reference_joints} + return {spec.child_role for spec in profile.measurement.measurements.values() if spec.parent_role in parents} + + +@dataclass(frozen=True) +class ObserverBridgeInput: + joint: str + parent_model: object + parent_report: dict + rows: tuple + endpoint_rows: tuple + held_channels: tuple + + +def build_observer_bridge(profile, source): + spec = profile.measurement.measurements[source.joint] + parent_model = source.parent_model + rows, held = source.rows, source.held_channels + if len(parent_model.geometry) != 1 or parent_model.geometry[0].relation.child_role != spec.parent_role: + raise ValueError('observer_parent_model_invalid') + target = profile.motion.joint_zero_references[source.joint].command + minimum = max(10, profile.acquisition.fixed_reference_minimum_frames) + endpoints = source.endpoint_rows[-minimum:] + if len(rows) < 24 or len(endpoints) < minimum or not held: + raise ValueError('observer_images_missing') + if (max(r['image_stamp_ns'] for r in rows) >= min(r['image_stamp_ns'] for r in endpoints) + or endpoints[-1]['image_stamp_ns']-endpoints[0]['image_stamp_ns'] < 200_000_000): + raise ValueError('observer_current_images_missing') + combined = rows+endpoints + if (any(r['session_epoch'] != source.parent_report['session_epoch'] for r in rows) + or any(r['session_epoch'] != endpoints[0]['session_epoch'] for r in endpoints)): + raise ValueError('observer_source_epoch_changed') + if any(r.get('command_vector') is None or r.get('feedback_vector') is None + or r['view'] != spec.view or r['sample_phase'] != 'zero_approach' + or any(r['command_vector'][i] != target[i] for i in held) for r in combined): + raise ValueError('observer_held_posture_changed') + feedback = np.asarray([r['feedback_vector'] for r in combined])[:, held] + if not np.all(np.isfinite(feedback)) or np.any(np.ptp(feedback, axis=0) > feedback_tolerances(profile.command.unit).stability): + raise ValueError('observer_distal_feedback_changed') + parent_frames = bridge_frames(rows, parent_model.geometry[0].relation) + parents, candidates, corners, matrices, sizes = [], [], [], [], set() + expected_size = next(tag.size_m for view in profile.vision.views if view.name == spec.view + for tag in view.tags if tag.role == spec.child_role) + for row, frame in zip(rows, parent_frames): + if frame.camera_matrix != parent_model.camera_matrix: + raise ValueError('observer_camera_changed') + selected = select_image_motion_frame(parent_model, frame) + if not selected.resolved: + raise ValueError('observer_parent_pose_unresolved') + parents.append(next(s.pose for s in selected.selections if s.role == spec.parent_role)) + tag = row['tags'][spec.child_role] + if (tag['candidate_diagnostics']['observation_stamp_ns'] != row['image_stamp_ns'] + or tag['tag_size_m'] != expected_size): + raise ValueError('observer_image_identity_changed') + candidates.append(tuple(_pose(p) for p in tag['reprojection_valid_candidates'])) + corners.append(tag['corners_xy']) + sizes.add(tag['tag_size_m']) + matrices.append(row['camera_matrix']) + if len(sizes) != 1: + raise ValueError('observer_tag_size_changed') + uncertainty = {name: (angle, point) for name, angle, point in source.parent_report['geometry_uncertainty']} + inherited = uncertainty[parent_model.geometry[0].relation.joint] + pose = fit_observer_pose(parents, candidates, corners, matrices, sizes.pop(), + tuple(r['image_stamp_ns'] for r in rows), inherited=inherited, + parent_corners=[r['tags'][spec.parent_role]['corners_xy'] for r in rows], + parent_size=dict(parent_model.tag_sizes)[spec.parent_role]) + relation = MotionRelation(source.joint, spec.parent_role, spec.child_role) + bridge = SharedTagPoseBridge(relation, pose.quaternion_xyzw, pose.translation_xyz_m, + bridge_frames(endpoints, relation), rotation_uncertainty_rad=pose.rotation_uncertainty_rad, + translation_uncertainty_m=pose.translation_uncertainty_m) + record = dict(joint=source.joint, version='observer_pose_bridge_v1', + parent_model_sha256=source.parent_report['image_model_sha256'], + parent_evidence_ids=source.parent_report['evidence_ids'], held_channels=list(held), pose=asdict(pose), + source_frame_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in rows}, + endpoint_frame_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in endpoints}) + return bridge, record + + +def observer_inputs(owner, view, endpoint_rows, arc_stamps, epoch): + result = [] + if owner.tag_feedback_channels is None or owner.profile.vision_motion: + return () + for joint in owner.zero_joints: + spec = owner.profile.measurement.measurements[joint] + if spec.view != view: + continue + sources = {(report['image_model_sha256']): (model, report) + for (source_epoch, source_view, _), (model, report) in owner.parent_references.models.items() + if source_epoch == epoch and source_view == view and model.image_model is not None + and len(model.image_model.geometry) == 1 + and model.image_model.geometry[0].relation.child_role == spec.parent_role} + for model, report in sorted(sources.values(), key=lambda pair: max(pair[1]['source_image_stamps']), reverse=True): + observed = owner.observer_records.get((report['session_epoch'], report['task_name'], view), {}) + rows = tuple(observed[stamp] for stamp in sorted(report['source_image_stamps']) + if stamp in observed and spec.child_role in observed[stamp]['tags']) + endpoints = tuple(r for r in endpoint_rows if r['image_stamp_ns'] not in arc_stamps) + if len(rows) < 24 or len(endpoints) < 10: + continue + held = tuple(sorted(owner.tag_feedback_channels[spec.child_role]-owner.tag_feedback_channels[spec.parent_role])) + result.append(ObserverBridgeInput(joint, model.image_model, report, rows, endpoints, held)) + return tuple(result) + + +def restore_observer_records(owner, records): + """Restore only the bounded source frames of already verified models.""" + selected = {} + for _, report in owner.parent_references.models.values(): + key = report['session_epoch'], report['task_name'], report['view'] + selected.setdefault(key, set()).update(report['source_image_stamps']) + for row in records: + if row.get('kind') != 'motion_branch_observation': + continue + key = row['session_epoch'], row['task_name'], row['view'] + if row['image_stamp_ns'] in selected.get(key, ()): + owner.observer_records.setdefault(key, {})[row['image_stamp_ns']] = row + + +def validate_observer_channels(profile, records, source_model): + from ..profiles.observations import compile_tag_feedback_channels + channels = compile_tag_feedback_channels(profile, source_model) + for row in records: + for record in row.get('observer_pose_bridges', ()): + spec = profile.measurement.measurements[record['joint']] + expected = sorted(channels[spec.child_role]-channels[spec.parent_role]) + if list(record['held_channels']) != expected: + raise ValueError('observer_held_channels_changed') + + +def validate_observer_references(profile, records, evidence): + """Reconstruct the measured bridge from its bound raw images, never cache.""" + from ..core.geometry.image_motion_replay import frozen_image_model + from ..core.geometry.tag_pose.pose_bridge import check_pose_bridge + from ..core.fitting.motion_fit import channel_for_joint + from .parent_reference_evidence import source_epoch_matches + observations = {(r['view'], str(r['image_stamp_ns'])): r for r in records + if r.get('kind') == 'motion_branch_observation'} + checked = set() + for payload in evidence.values(): + for record in payload.get('observer_pose_bridges', ()): + identity = source_frame_sha256(record), payload['image_model_sha256'] + if identity in checked: + continue + checked.add(identity) + joint = record['joint'] + spec = profile.measurement.measurements[joint] + parent_id = record['parent_evidence_ids'].get(spec.parent_role) + parent = evidence.get(parent_id) + if (parent is None or parent.get('image_model_sha256') != record['parent_model_sha256'] + or parent['view'] != payload['view'] or parent['tag_role'] != spec.parent_role): + raise ValueError('observer_parent_installation_changed') + held = tuple(record['held_channels']) + expected = (channel_for_joint(profile, joint),) + if held != expected: + raise ValueError('observer_held_channels_changed') + groups = [] + eligible = {str(stamp) for stamp in parent['source_image_stamps'] + if (spec.view, str(stamp)) in observations + and spec.child_role in observations[(spec.view, str(stamp))].get('tags', {})} + if set(record['source_frame_hashes']) != eligible: + raise ValueError('observer_source_sampling_changed') + for field in ('source_frame_hashes', 'endpoint_frame_hashes'): + rows = [] + for stamp, digest in record[field].items(): + row = observations.get((spec.view, stamp)) + if row is None or source_frame_sha256(row) != digest: + raise ValueError('observer_source_pixels_changed') + if field == 'source_frame_hashes': + if (int(stamp) not in parent['source_image_stamps'] + or row['task_name'] != parent['task_name'] + or row['session_epoch'] != parent['session_epoch']): + raise ValueError('observer_parent_image_binding_changed') + elif (int(stamp) in payload['source_image_stamps'] + or row['task_name'] != payload['task_name'] + or row['session_epoch'] != payload['session_epoch'] + or row['motion_version'] != payload['motion_version']): + raise ValueError('observer_endpoint_binding_changed') + rows.append(row) + groups.append(tuple(sorted(rows, key=lambda r: r['image_stamp_ns']))) + target = profile.motion.joint_zero_references[joint].command + endpoint_candidates = sorted((r for r in observations.values() + if all(r.get(k) == payload[k] for k in ('view', 'task_name', 'session_epoch', 'motion_version', 'zero_joints')) + and tuple(r.get('command_vector') or ()) == target and r.get('sample_phase') == 'zero_approach' + and r['image_stamp_ns'] not in payload['source_image_stamps'] + and r['image_stamp_ns'] <= max(payload['source_image_stamps'])), key=lambda r: r['image_stamp_ns']) + expected_endpoints = endpoint_candidates[-max(10, profile.acquisition.fixed_reference_minimum_frames):] + if [r['image_stamp_ns'] for r in groups[1]] != [r['image_stamp_ns'] for r in expected_endpoints]: + raise ValueError('observer_endpoint_sampling_changed') + parent_report = {**parent, 'evidence_ids': record['parent_evidence_ids']} + if not groups[1] or not source_epoch_matches(records, parent, payload['session_epoch'], parent_id, + groups[1][0]['image_stamp_ns']): + raise ValueError('observer_source_epoch_changed') + for role, evidence_id in record['parent_evidence_ids'].items(): + if (evidence_id not in evidence or evidence[evidence_id]['tag_role'] != role + or evidence[evidence_id]['image_model_sha256'] != record['parent_model_sha256']): + raise ValueError('observer_parent_identity_changed') + bridge, reproduced = build_observer_bridge(profile, ObserverBridgeInput(joint, + frozen_image_model(parent), parent_report, *groups, held)) + if source_frame_sha256(reproduced) != source_frame_sha256(record): + raise ValueError('observer_pose_evidence_changed') + if check_pose_bridge(frozen_image_model(payload), bridge).status != 'consistent': + raise ValueError('observer_current_model_inconsistent') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py index 0c56e27..35b85de 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py @@ -68,6 +68,8 @@ class ParentReferenceRegistry: self.clear() def clear(self): + from .shared_pose_handoff import SharedPoseHandoff + self.handoff = SharedPoseHandoff(self.profile) self.models = {} self.verified = {} self.zero_references = {} @@ -92,6 +94,7 @@ class ParentReferenceRegistry: """ if current != previous+1: raise ValueError("parent_reference_recovery_epoch_discontinuous") + self.handoff.advance_recovery_epoch(previous, current) self.models.update({(current, view, joint): value for (epoch, view, joint), value in tuple(self.models.items()) if epoch == previous}) self.verified.update({(current, task): value @@ -109,13 +112,43 @@ class ParentReferenceRegistry: return pose, geometry def activate(self, task, epoch, capture): - (model, _), _ = self.sources(task, epoch) + model, _ = self.pose_source(task, epoch) self.verified.pop((epoch, task.key), None) capture.activate_reference_model(model) + def pose_source(self, task, epoch): + """Use the latest explicitly handed-off model for the same rigid Tag. + + Keep comparing against the ORIGINAL pose joint's measured zero. This + changes the image model, never the accepted reference or its bounds. + """ + from ..core.geometry.tag_pose.pose_bridge import is_handoff_policy + pose, geometry = self.sources(task, epoch) + if not is_handoff_policy(geometry[1].get('image_model_selection_policy')): + return pose + bridges = geometry[1].get('shared_tag_pose_bridges', ()) + if not any(b.get('joint') == task.parent_reference.geometry_joint + and b.get('source_reference', {}).get('joint') == task.parent_reference.pose_joint + and b.get('source_model_sha256') == pose[1]['image_model_sha256'] for b in bridges): + raise ValueError('parent_reference_handoff_source_changed') + return geometry + + def activate_joints(self, joints, epoch, capture): + installed = set() + for joint in joints: + spec = self.profile.measurement.measurements[joint] + try: + model, report = self.models[(epoch, spec.view, joint)] + except KeyError as error: + raise ValueError(f"revisit_frozen_model_missing:{joint}") from error + identity = report["image_model_sha256"] + if identity not in installed: + capture.activate_reference_model(model) + installed.add(identity) + def confirm(self, task, epoch, version, reference, rows): from .motion_provenance import source_frame_sha256 - (model, report), _ = self.sources(task, epoch) + model, report = self.pose_source(task, epoch) role = self.profile.measurement.measurements[task.parent_reference.pose_joint].child_role check_parent_pose(self.profile, task, reference, rows, self.channels[role]) identity = dict(model.evidence_ids)[role] @@ -129,6 +162,10 @@ class ParentReferenceRegistry: "pose_joint": reference.joint, "pose_evidence_id": identity, "source_reference": reference.as_record(), "held_channels": sorted(self.channels[role]), "source_frame_hashes": {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in rows}} + from ..core.geometry.tag_pose.pose_bridge import is_handoff_policy + if is_handoff_policy(report.get('image_model_selection_policy')): + record.update(schema_version=2, pose_model_joint=task.parent_reference.geometry_joint, + pose_model_policy='shared_pose_handoff_parent_v1') self.verified[(epoch, task.key)] = record return record diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py index 2d1fb6d..ae5a83a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py @@ -32,8 +32,12 @@ def source_epoch_matches(records, source, target_epoch, identity, target_stamp): if same_capture_epoch(records, source["session_epoch"], target_epoch, source_stamp, target_stamp): return True for row in records: - if (row.get("kind") != "passed_pose_models_restored" or row.get("resume_mode") != "passed" - or row.get("unchanged_installation_declared") is not True + authorized = (row.get("kind") == "passed_pose_models_restored" and row.get("resume_mode") == "passed" + and row.get("unchanged_installation_declared") is True) + authorized = authorized or (row.get("kind") == "staged_pose_models_restored" and row.get("resume_mode") == "verify" + and row.get("physical_zero_reverification_required") is True + and any(r.get("kind") == "resume_verification" and r.get("reuse") is True for r in records)) + if (not authorized or not source_stamp < row.get("stamp_ns", 0) < target_stamp or not same_capture_epoch(records, row.get("session_epoch"), target_epoch, row["stamp_ns"], target_stamp)): @@ -109,6 +113,21 @@ def validate_parent_references(profile, records, evidence, *, source_model=None) or not source_epoch_matches(records, pose_source, check["session_epoch"], check["pose_evidence_id"], min(map(int, check.get("source_frame_hashes", {"0": ""}))))): raise ValueError("parent_reference_pose_source_unbound") + from ..core.geometry.tag_pose.pose_bridge import is_handoff_policy + if is_handoff_policy(pose_source.get('image_model_selection_policy')) != (check.get('schema_version', 1) == 2): + raise ValueError('parent_reference_handoff_policy_changed') + if check.get('schema_version', 1) == 2: + if (check.get('pose_model_policy') != 'shared_pose_handoff_parent_v1' + or check.get('pose_model_joint') != task.parent_reference.geometry_joint + or not is_handoff_policy(pose_source.get('image_model_selection_policy')) + or not any(b.get('joint') == task.parent_reference.geometry_joint + and b.get('source_reference', {}).get('joint') == name + for b in pose_source.get('shared_tag_pose_bridges', ())) + or any(s.model_sha256 != pose_source['image_model_sha256'] for s in model.source_hinges)): + raise ValueError('parent_reference_handoff_source_changed') + elif (check.get('schema_version', 1) != 1 or 'pose_model_policy' in check + or 'pose_model_joint' in check): + raise ValueError('parent_reference_handoff_policy_changed') rows = [] for stamp, digest in check.get("source_frame_hashes", {}).items(): row = samples.get((spec.view, stamp)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_samples.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_samples.py new file mode 100644 index 0000000..c383d33 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_samples.py @@ -0,0 +1,55 @@ +"""Admit synchronized preparation frames without hiding changed observations.""" +import numpy as np + +ADMISSION_POLICY = 'bounded_preparation_sync_v1' + + +def admit_preparation_rows(profile, rows, *, held_channels, target, group_key): + """Keep raw rows intact; missing values never become fabricated commands. + + Every available command/feedback is checked, including rejected images. + Missing samples must be bracketed within the same motion leg, occupy at + most the existing unobserved fraction, and span at most one steady window. + """ + from .motion_execution import feedback_tolerances + complete, rejected, feedback = [], [], [] + for row in rows: + missing = [] + for field in ('command_vector', 'feedback_vector'): + value = row.get(field) + if value is None: + missing.append(field) + continue + vector = np.asarray(value, dtype=float) + if vector.shape != (profile.command.command_count,) or not np.all(np.isfinite(vector)): + raise ValueError('preparation_state_vector_invalid') + if field == 'command_vector' and any(vector[i] != target[i] for i in held_channels): + raise ValueError('preparation_witness_held_posture_changed') + if field == 'feedback_vector': + feedback.append(vector) + if missing: + rejected.append({'image_stamp_ns': row['image_stamp_ns'], 'missing': missing}) + else: + complete.append(row) + if feedback and np.any(np.ptp(np.asarray(feedback)[:, held_channels], axis=0) + > feedback_tolerances(profile.command.unit).stability): + raise ValueError('preparation_witness_held_posture_changed') + if not rejected: + return tuple(complete), () + if len(rejected)/len(rows) > profile.acquisition.maximum_unobserved_fraction: + raise ValueError('preparation_sync_coverage_missing') + valid = {r['image_stamp_ns'] for r in complete} + for group in dict.fromkeys(r[group_key] for r in rows): + leg = [r for r in rows if r[group_key] == group] + if leg[0]['image_stamp_ns'] not in valid or leg[-1]['image_stamp_ns'] not in valid: + raise ValueError('preparation_sync_endpoint_missing') + previous, missing = leg[0]['image_stamp_ns'], False + for row in leg[1:]: + stamp = row['image_stamp_ns'] + if stamp not in valid: + missing = True + else: + if missing and stamp-previous > int(profile.acquisition.steady_window_seconds*1e9): + raise ValueError('preparation_sync_gap_too_long') + previous, missing = stamp, False + return tuple(complete), tuple(rejected) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition.py new file mode 100644 index 0000000..70b9103 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition.py @@ -0,0 +1,248 @@ +"""Reuse a recorded single-channel posture change between shared-Tag tasks. + +This collector never commands motion and never retries a candidate. It retains +one bounded entry leg, before the current joint's own preparation. Selection is +by command bins and endpoint time, independently of fitting residuals. +""" + +from dataclasses import dataclass +import math + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.domain.reference import JointZeroReference +from ..core.geometry.image_motion_replay import frozen_image_model +from ..core.geometry.tag_pose.motion_evidence import MotionRelation +from ..core.geometry.tag_pose.image_motion_projection import select_image_motion_frame +from ..core.geometry.tag_pose.relative import _relative_pose +from ..core.geometry.tag_pose.transition_image_solver import PreparationTransition +from .shared_tag_reference import bridge_frames +from .motion_execution import feedback_tolerances +from .motion_provenance import source_frame_sha256 + + +class PreparationTransitionUnavailable(ValueError): + """Optional entry sampling is incomplete; no transition has been fitted.""" + + +@dataclass(frozen=True) +class TransitionSource: + joint: str + transition_joint: str + channel: int + relevant_channels: tuple + reference: JointZeroReference + source_report: dict + rows: tuple = () + + +def transition_source(profile, joint, epoch, registry, channels): + if channels is None or profile.vision_motion: + return None + from ..core.fitting.motion_fit import channel_for_joint + spec = profile.measurement.measurements[joint] + if not any(t.role == spec.parent_role and t.fixed_reference + for view in profile.vision.views if view.name == spec.view for t in view.tags): + return None + relevant = tuple(sorted(channels[spec.parent_role] | channels[spec.child_role])) + target = profile.motion.joint_zero_references[joint].command + candidates = [] + for (source_epoch, view, name), reference in registry.zero_references.items(): + previous = profile.measurement.measurements[name] + if (source_epoch != epoch or name == joint or view != spec.view + or (previous.parent_role, previous.child_role) != (spec.parent_role, spec.child_role)): + continue + samples = [s for s in reference.samples if s.view == view] + if not samples: + continue + changed = [i for i in relevant if samples[0].command[i] != target[i]] + if len(changed) != 1 or changed[0] == channel_for_joint(profile, joint): + continue + names = [name for name in profile.measurement.measurements + if channel_for_joint(profile, name) == changed[0]] + source = registry.models.get((epoch, view, reference.joint)) + if (len(names) != 1 or source is None or source[0].image_model is None + or len(source[0].image_model.geometry) != 1): + continue + candidates.append(TransitionSource(joint, names[0], changed[0], relevant, reference, source[1])) + return max(candidates, key=lambda c: max(s.image_stamp_ns for s in c.reference.samples)) if candidates else None + + +class PreparationTransitionCapture: + """One entry leg per task visit; resets on a changed task, epoch or reuse.""" + + def __init__(self, owner): + self.owner = owner + self.clear() + + def clear(self): + self.identity = None + self.source = None + self.rows = [] + self.version = None + self.error = "" + + def begin(self, motion, epoch): + if motion.phase not in {'zero_approach', 'joint_zero'} or motion.reference_reuse or motion.reference_check: + self.clear() + return + joints = motion.reference_joints if motion.phase == 'zero_approach' else motion.zero_joints + identity = motion.task_key, tuple(joints), epoch + if identity == self.identity: + return + self.clear() + self.identity = identity + if len(joints) == 1 and not motion.reference_only: + self.source = transition_source(self.owner.profile, joints[0], epoch, + self.owner.parent_references, self.owner.tag_feedback_channels) + + def observe(self, row): + if self.source is None or self.error: + return + spec = self.owner.profile.measurement.measurements[self.source.joint] + if (row.get('view') != spec.view or row.get('sample_phase') != 'zero_approach' + or (row.get('task_name'), tuple(row.get('zero_joints', ())), row.get('session_epoch')) != self.identity + or row.get('command_vector') is None or row.get('feedback_vector') is None): + return + channel = self.source.channel + before = next(s.command[channel] for s in self.source.reference.samples if s.view == spec.view) + if self.version is None: + if row['command_vector'][channel] != before: + return + self.version = row['motion_version'] + if row['motion_version'] != self.version: + return + if self.rows and row['image_stamp_ns'] <= self.rows[-1]['image_stamp_ns']: + self.error = 'preparation_transition_image_identity_changed' + elif len(self.rows) >= 512: + self.error = 'preparation_transition_frame_limit' + else: + self.rows.append(row) + + def snapshot(self, view): + from dataclasses import replace + if self.source is None or self.owner.profile.measurement.measurements[self.source.joint].view != view: + return None + if self.error: + raise ValueError(self.error) + # Absent optional entry data never create an invented bridge. + if not self.rows: + return None + return replace(self.source, rows=tuple(self.rows)) + + +def select_transition_rows(profile, rows, channel, before, after): + lower, upper = profile.command.minimum_values[channel], profile.command.maximum_values[channel] + bins = {} + for row in rows: + bins[round(128*(row['command_vector'][channel]-lower)/(upper-lower))] = row + starts = [r for r in rows if r['command_vector'][channel] == before][-10:] + endings = [r for r in rows if r['command_vector'][channel] == after][-10:] + selected = {r['image_stamp_ns']: r for r in (*bins.values(), *starts, *endings)} + return tuple(selected[k] for k in sorted(selected)), tuple(starts), tuple(endings) + + +def build_preparation_transition(profile, source, current_rows): + """Live and offline admission share physical, timing and identity checks.""" + from ..core.fitting.motion_fit import channel_for_joint + spec = profile.measurement.measurements[source.joint] + target = profile.motion.joint_zero_references[source.joint].command + rows = source.rows + old = tuple(s for s in source.reference.samples if s.view == spec.view) + if len(rows) < 24: + raise PreparationTransitionUnavailable('preparation_transition_samples_missing') + if len(old) < 10 or len(rows) > 512 or len(current_rows) < 24: + raise ValueError('preparation_transition_samples_missing') + parent_model = frozen_image_model(source.source_report) + identity = source.source_report['evidence_ids'].get(spec.child_role) + if (not any(b.view == spec.view and b.tag_role == spec.child_role and b.motion_evidence_id == identity + for b in source.reference.branch_references) + or len(parent_model.geometry) != 1 + or (parent_model.relations[0].parent_role, parent_model.relations[0].child_role) + != (spec.parent_role, spec.child_role)): + raise ValueError('preparation_transition_source_model_changed') + channel = source.channel + before, after = old[0].command[channel], target[channel] + held = tuple(i for i in source.relevant_channels if i != channel) + if (before == after or any(s.command[i] != target[i] for s in old for i in held) + or any(s.command[channel] != before for s in old)): + raise ValueError('preparation_transition_held_posture_changed') + stamps = [r['image_stamp_ns'] for r in rows] + current = sorted(current_rows, key=lambda r: r['image_stamp_ns']) + if (stamps != sorted(set(stamps)) or min(stamps) <= max(s.image_stamp_ns for s in old) + or max(stamps) >= current[0]['image_stamp_ns'] + or len({r['motion_version'] for r in rows}) != 1 + or rows[-1]['motion_version'] >= current[0]['motion_version']): + raise ValueError('preparation_transition_order_changed') + for row in (*rows, *current): + if (row.get('kind') != 'motion_branch_observation' or row.get('sample_phase') != 'zero_approach' + or row.get('cycle') == 3 or row['view'] != spec.view or row.get('input_is_rectified') is not True + or any(row.get(k) != current[0].get(k) for k in ('task_name', 'zero_joints', 'session_epoch')) + or row.get('command_vector') is None or row.get('feedback_vector') is None): + raise ValueError('preparation_transition_capture_identity_changed') + if any(any(r['command_vector'][i] != target[i] for i in held) for r in rows): + raise ValueError('preparation_transition_held_posture_changed') + commands = np.asarray([r['command_vector'][channel] for r in rows]) + if (commands[0] != before or commands[-1] != after + or np.any(np.diff(commands)*np.sign(after-before) < 0) + or np.any(commands < min(before, after)) or np.any(commands > max(before, after))): + raise ValueError('preparation_transition_path_changed') + feedback = np.asarray([r['feedback_vector'] for r in rows]) + old_feedback = np.asarray([s.feedback for s in old]) + tolerance = feedback_tolerances(profile.command.unit).stability + if np.ptp(feedback[:, channel]) < feedback_tolerances(profile.command.unit).minimum_travel: + raise ValueError('preparation_transition_feedback_did_not_move') + if (not np.all(np.isfinite(feedback)) or np.any(np.ptp(feedback[:, held], axis=0) > tolerance) + or np.any(np.abs(feedback[:, held]-np.median(old_feedback[:, held], axis=0)) > 2*tolerance)): + raise ValueError('preparation_transition_held_feedback_changed') + selected, starts, endings = select_transition_rows(profile, rows, channel, before, after) + if not starts or len(endings) < 10: + raise PreparationTransitionUnavailable('preparation_transition_endpoint_samples_missing') + if (endings[-1]['image_stamp_ns']-endings[0]['image_stamp_ns'] < 200_000_000 + or np.ptp([r['feedback_vector'][channel] for r in endings]) > tolerance + or any(abs(r['feedback_vector'][channel]-np.median(old_feedback[:, channel])) > 2*tolerance for r in starts)): + raise ValueError('preparation_transition_endpoints_unstable') + main_channel = channel_for_joint(profile, source.joint) + current_held = tuple(i for i in source.relevant_channels if i != main_channel) + ending_feedback = np.median([r['feedback_vector'] for r in endings], axis=0) + if any(r['command_vector'][i] != target[i] + or abs(r['feedback_vector'][i]-ending_feedback[i]) > 2*tolerance + for r in current for i in current_held): + raise ValueError('preparation_transition_current_posture_changed') + current_zeros = tuple(r['image_stamp_ns'] for r in current + if r['command_vector'][main_channel] == target[main_channel] + and abs(r['feedback_vector'][main_channel]-ending_feedback[main_channel]) <= tolerance) + relation = MotionRelation(source.transition_joint, spec.parent_role, spec.child_role) + if any(not all(role in r['tags'] for role in (spec.parent_role, spec.child_role)) for r in selected): + raise PreparationTransitionUnavailable('preparation_transition_pixels_missing') + frames = bridge_frames(selected, relation) + if any(f.camera_matrix != parent_model.camera_matrix or f.reference_frames != parent_model.reference_frames for f in frames): + raise ValueError('preparation_transition_camera_reference_changed') + rotation = Rotation.from_quat([s.relative_quaternion_xyzw for s in old]) + translation = np.asarray([s.relative_translation_xyz_m for s in old]) + if (np.max((rotation.mean().inv()*rotation).magnitude()) > math.radians(1) + or np.max(np.linalg.norm(translation-np.median(translation, axis=0), axis=1)) > .001): + raise ValueError('preparation_transition_source_zero_unstable') + point, orientation = np.median(translation, axis=0), rotation.mean() + # An old branch is rechecked on the actual, later shared starting posture. + # This does not authorize a new zero from these few stationary frames. + for frame in bridge_frames(starts, relation): + checked = select_image_motion_frame(parent_model, frame) + poses = {s.role: s.pose for s in checked.selections} + if not checked.resolved: + raise ValueError('preparation_transition_source_image_unresolved') + turn, position = _relative_pose(poses[spec.parent_role], poses[spec.child_role]) + if (orientation.inv()*turn).magnitude() > math.radians(2) or np.linalg.norm(position-point) > .005: + raise ValueError('preparation_transition_source_pose_changed') + transition = PreparationTransition(relation, frames, + tuple(0 if r['command_vector'][channel] == before else 1 if r['command_vector'][channel] == after else -1 for r in selected), + tuple(orientation.as_quat()), tuple(point), current_zeros) + record = dict(version='preparation_transition_v1', joint=source.joint, + transition_joint=source.transition_joint, channel=channel, relevant_channels=list(source.relevant_channels), + source_reference=source.reference.as_record(), source_evidence_id=identity, + source_model_sha256=source.source_report['image_model_sha256'], + source_frame_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in rows}, + selected_image_stamps=[r['image_stamp_ns'] for r in selected], + current_zero_stamps=list(current_zeros)) + return transition, record diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition_evidence.py new file mode 100644 index 0000000..b3b936e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_transition_evidence.py @@ -0,0 +1,123 @@ +"""Audit transition-conditioned preparation from immutable original records.""" + +from types import SimpleNamespace + +from ..core.domain.reference import JointZeroReference, JointZeroSample +from ..core.geometry.image_motion_replay import frozen_image_model, replay_image_models, validate_image_sample +from ..core.geometry.tag_pose.image_motion_model import image_model_payload +from ..core.geometry.tag_pose.transition_image_solver import TRANSITION_SELECTION_POLICY, resolve_transition_motion +from .motion_provenance import source_frame_sha256 +from .preparation_transition import TransitionSource, build_preparation_transition, transition_source +from .shared_tag_reference import bridge_frames + + +def check_transition_report(row): + transitions = row.get('preparation_transitions', ()) + if (row.get('image_model_selection_policy') == TRANSITION_SELECTION_POLICY) != bool(transitions): + raise ValueError('preparation_transition_selection_policy_changed') + if not transitions: + return + if len(transitions) != 1 or row.get('shared_tag_pose_bridges') or row.get('observer_pose_bridges'): + raise ValueError('preparation_transition_composition_changed') + selected = row.get('selected_hypothesis') + hypotheses = row.get('hypotheses', ()) + if type(selected) is not int or not 0 <= selected < len(hypotheses): + raise ValueError('preparation_transition_winner_missing') + winner = hypotheses[selected] + model = frozen_image_model(row) + if (winner.get('training_accepted') is not True or winner.get('converged') is not True + or winner.get('reason') or not winner.get('transition_branches') + or tuple(map(tuple, winner['branches'])) != model.branches + or row.get('geometry_uncertainty') != winner.get('child_frame_uncertainty') + or any(q['maximum_frame_rms_px'] > model.maximum_reprojection_error_px + for q in (*winner['training_quality'], *winner['validation_quality']))): + raise ValueError('preparation_transition_winner_changed') + + +def transition_inputs(records, evidence): + references = [r for r in records if r.get('kind') == 'joint_zero_reference'] + references.extend(r['current_reference'] for r in records if r.get('kind') == 'joint_zero_motion_reference_verified') + reference_hashes = {source_frame_sha256(r) for r in references} + observations = {(r['view'], str(r['image_stamp_ns'])): r for r in records + if r.get('kind') == 'motion_branch_observation'} + checked = set() + for payload in evidence.values(): + for record in payload.get('preparation_transitions', ()): + key = payload['image_model_sha256'], source_frame_sha256(record) + if key in checked: + continue + checked.add(key) + if record.get('version') != 'preparation_transition_v1': + raise ValueError('preparation_transition_version_changed') + ref = record.get('source_reference', {}) + if source_frame_sha256(ref) not in reference_hashes: + raise ValueError('preparation_transition_original_zero_missing') + reference = JointZeroReference.from_record(ref) + source = evidence.get(record.get('source_evidence_id')) + if (source is None or source.get('image_model_sha256') != record.get('source_model_sha256') + or source['view'] != payload['view'] or source['tag_role'] != payload['tag_role'] + or reference.joint not in source['zero_joints'] or record['joint'] not in payload['zero_joints']): + raise ValueError('preparation_transition_source_unbound') + rows = [] + for stamp, digest in record.get('source_frame_hashes', {}).items(): + row = observations.get((payload['view'], stamp)) + if row is None or source_frame_sha256(row) != digest: + raise ValueError('preparation_transition_source_pixels_changed') + rows.append(row) + rows.sort(key=lambda r: r['image_stamp_ns']) + if not rows: + raise ValueError('preparation_transition_sources_missing') + # Omitting inconvenient raw frames cannot change bin selection or + # hide a changed holding channel between selected observations. + complete = [r for r in observations.values() + if all(r.get(k) == rows[0].get(k) for k in ('task_name', 'view', 'zero_joints', 'session_epoch', 'motion_version')) + and r.get('command_vector') is not None and r.get('feedback_vector') is not None] + if {r['image_stamp_ns'] for r in complete} != {r['image_stamp_ns'] for r in rows}: + raise ValueError('preparation_transition_window_incomplete') + from .parent_reference_evidence import source_epoch_matches + if not source_epoch_matches(records, source, payload['session_epoch'], + record['source_evidence_id'], rows[0]['image_stamp_ns']): + raise ValueError('preparation_transition_source_epoch_changed') + report = dict(source, evidence_ids={payload['tag_role']: record['source_evidence_id']}) + prepared = TransitionSource(record['joint'], record['transition_joint'], record['channel'], + tuple(record['relevant_channels']), reference, report, tuple(rows)) + current = tuple(observations[(payload['view'], str(stamp))] for stamp in payload['source_image_stamps']) + yield payload, record, prepared, current + + +def validate_transition_topology(profile, records, evidence, source_model): + from ..profiles.observations import compile_tag_feedback_channels + channels = compile_tag_feedback_channels(profile, source_model) + for payload, record, prepared, _ in transition_inputs(records, evidence): + epoch = payload['session_epoch'] + registry = SimpleNamespace(zero_references={(epoch, payload['view'], prepared.reference.joint): prepared.reference}, + models={(epoch, payload['view'], prepared.reference.joint): + (SimpleNamespace(image_model=frozen_image_model(prepared.source_report)), prepared.source_report)}) + expected = transition_source(profile, record['joint'], epoch, registry, channels) + if (expected is None or (expected.channel, expected.transition_joint, expected.relevant_channels) + != (prepared.channel, prepared.transition_joint, prepared.relevant_channels)): + raise ValueError('preparation_transition_topology_changed') + + +def validate_preparation_transitions(profile, records, evidence): + samples = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind') == 'joint_zero_sample'} + pixels = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind') == 'pnp_candidate_frame'} + for payload, record, prepared, current in transition_inputs(records, evidence): + for sample in prepared.reference.samples: + row = samples.get((sample.view, sample.image_stamp_ns)) + frame = pixels.get((sample.view, sample.image_stamp_ns)) + if (row is None or frame is None or row.get('joint') != prepared.reference.joint + or JointZeroSample.from_row(row, profile.command.unit) != sample): + raise ValueError('preparation_transition_zero_pixels_missing') + validate_image_sample(row, frame, replay_image_models(frame)) + transition, reconstructed = build_preparation_transition(profile, prepared, current) + if source_frame_sha256(reconstructed) != source_frame_sha256(record): + raise ValueError('preparation_transition_sampling_changed') + model = frozen_image_model(payload) + frames = bridge_frames(current, model.relations[0]) + from ..core.geometry.tag_pose.production_image_motion import image_parameters_for_sampling + result = resolve_transition_motion(frames, model.relations, transition, + parameters=image_parameters_for_sampling(payload.get('image_sampling_policy'))) + if (not result.resolved + or source_frame_sha256(image_model_payload(result.model)) != payload['image_model_sha256']): + raise ValueError('preparation_transition_frozen_model_changed') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness.py new file mode 100644 index 0000000..4e1ffcc --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness.py @@ -0,0 +1,298 @@ +"""Independent same-Tag pose witnesses at a task's declared avoidance posture. + +No commands, retries, SDK-to-angle conversion or validation-round samples live +here. Only the current finger is observed; other fingers retain their avoidance +commands. The source excitation is separate from the recipient preparation. +""" +from dataclasses import asdict, dataclass +import math + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.geometry.tag_pose.motion_evidence import MotionRelation +from ..core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge +from ..core.geometry.tag_pose.production_image_motion import ImageMotionParameters, resolve_image_motion +from ..core.geometry.tag_pose.image_motion_projection import select_image_motion_frame +from ..core.geometry.tag_pose.image_motion_model import image_model_payload +from ..core.geometry.tag_pose.relative import _relative_pose +from .shared_tag_reference import bridge_frames +from .motion_execution import feedback_tolerances +from .motion_provenance import source_frame_sha256 + +WITNESS_POLICY = 'training_independent_preparation_witness_holm_v1' +INDEPENDENT_WITNESS_POLICY = 'training_independent_witness_declined_v1' +MAXIMUM_WINDOW_FRAMES = 2048 + + +def witness_roles(profile, motion): + if motion.phase != 'zero_approach' or motion.reference_check or motion.reference_reuse: + return set() + donor = motion.preparation_witness_joint + if not donor: + return set() + specs = [profile.measurement.measurements[n] for n in (*motion.reference_joints, donor)] + return {role for spec in specs for role in (spec.parent_role, spec.child_role)} + + +@dataclass(frozen=True) +class WitnessInput: + joint: str + donor: str + rows: tuple + endpoint_rows: tuple + channels: tuple[int, ...] + + +class PreparationWitnessCapture: + """Bounded windows; sibling witnesses share the same immutable raw rows.""" + def __init__(self, owner): + self.owner = owner + self.clear() + + def clear(self): + self.windows = {} + self.paths = {} + + def observe(self, row): + profile = self.owner.profile + joints = tuple(row.get('zero_joints', ())) + if len(joints) != 1 or not row.get('preparation_witness_joint'): + return + witness = profile.motion.preparation_witnesses.get(joints[0]) + if (witness is None or witness.joint != row['preparation_witness_joint'] + or row.get('sample_phase') != 'zero_approach' or row.get('cycle') == 3 + or row['view'] != profile.measurement.measurements[joints[0]].view): + return + key = row['session_epoch'], row['task_name'], row['view'] + for previous in tuple(self.windows): + if previous[1:] == key[1:] and previous[0] < key[0]: + self.windows.pop(previous) + self.paths.pop(previous, None) + path = row.get('reference_path_index') + if path == 0 and self.paths.get(key, 0) > 0: + self.windows.pop(key, None) + if path is not None: + self.paths[key] = path + # Entering the first declared pose may move several holding channels. + if path is None or path < 1: + return + rows = self.windows.setdefault(key, []) + if len(rows) <= MAXIMUM_WINDOW_FRAMES: # One overflow marker, no unbounded growth. + rows.append(row) + + def inputs(self, view, joints, endpoints, arc_stamps, epoch): + result = [] + for joint in joints: + witness = self.owner.profile.motion.preparation_witnesses.get(joint) + spec = self.owner.profile.measurement.measurements[joint] + if witness is None or spec.view != view: + continue + task = self.owner.profile.motion.joint_zero_references[joint].task_key + windows = [(key, rows) for key, rows in self.windows.items() + if key[1:] == (task, view) and key[0] <= epoch] + rows = tuple(max(windows, key=lambda pair: pair[0][0])[1]) if windows else () + channels = self.owner.tag_feedback_channels + relevant = () if channels is None else tuple(sorted(channels[spec.parent_role] | channels[spec.child_role])) + result.append(WitnessInput(joint, witness.joint, rows, + tuple(r for r in endpoints if r['image_stamp_ns'] not in arc_stamps), relevant)) + return tuple(result) + + +def select_witness_rows(profile, source, *, legacy=False, admission=None): + """Deterministic command bins and stationary tail, before any image fitting.""" + from ..core.fitting.motion_fit import channel_for_joint + spec = profile.measurement.measurements[source.joint] + donor = profile.motion.preparation_witnesses[source.joint] + if donor.joint != source.donor: + raise ValueError("preparation_witness_dependency_changed") + task_key = profile.motion.joint_zero_references[source.joint].task_key + channel = channel_for_joint(profile, source.donor) + rows = source.rows + if not 24 <= len(rows) <= MAXIMUM_WINDOW_FRAMES or not source.channels or channel not in source.channels: + raise ValueError('preparation_witness_source_missing_or_oversized') + stamps = [r['image_stamp_ns'] for r in rows] + if stamps != sorted(set(stamps)): + raise ValueError('preparation_witness_image_identity_changed') + first = rows[0] + for row in rows: + if (row.get('kind') != 'motion_branch_observation' or row.get('sample_phase') != 'zero_approach' + or row.get('input_is_rectified') is not True or row.get('cycle') == 3 + or row.get('task_name') != task_key or row.get('view') != spec.view + or row.get('session_epoch') != first['session_epoch'] + or tuple(row.get('zero_joints', ())) != (source.joint,) + or row.get('preparation_witness_joint') != source.donor + or type(row.get('reference_path_index')) is not int + or not 1 <= row['reference_path_index'] <= len(donor.approach_commands) + or legacy and (row.get('command_vector') is None or row.get('feedback_vector') is None)): + raise ValueError('preparation_witness_capture_identity_changed') + held = tuple(i for i in source.channels if i != channel) + target = profile.motion.joint_zero_references[source.joint].command + if any(donor.command[i] != target[i] for i in source.channels): + raise ValueError('preparation_witness_posture_not_shared') + raw_rows = rows + if not legacy: + if any(r['camera_matrix'] != first['camera_matrix'] for r in rows): + raise ValueError('preparation_witness_camera_reference_changed') + from .preparation_samples import ADMISSION_POLICY, admit_preparation_rows + rows, rejected = admit_preparation_rows(profile, rows, held_channels=held, + target=target, group_key='reference_path_index') + if admission is not None: + admission.update(policy=ADMISSION_POLICY, rejected_frames=list(rejected)) + feedback = np.asarray([r['feedback_vector'] for r in rows], dtype=float) + commands = np.asarray([r['command_vector'] for r in rows], dtype=float) + tolerance = feedback_tolerances(profile.command.unit).stability + if (feedback.shape != (len(rows), profile.command.command_count) or commands.shape != feedback.shape + or not np.all(np.isfinite(feedback)) or not np.all(np.isfinite(commands)) + or any(np.any(commands[:, i] != target[i]) for i in held) + or np.any(np.ptp(feedback[:, held], axis=0) > tolerance)): + raise ValueError('preparation_witness_held_posture_changed') + if np.ptp(feedback[:, channel]) < feedback_tolerances(profile.command.unit).minimum_travel: + raise ValueError('preparation_witness_feedback_did_not_move') + paths = [r['reference_path_index'] for r in raw_rows] + if paths != sorted(paths) or set(paths) != set(range(1, len(donor.approach_commands)+1)): + raise ValueError('preparation_witness_path_incomplete') + for index in sorted(set(paths)): + values = [r['command_vector'][channel] for r in rows if r['reference_path_index'] == index] + before = donor.approach_commands[index-1][channel] + after = (donor.approach_commands[index] if index < len(donor.approach_commands) else donor.command)[channel] + if np.any(np.diff(values)*np.sign(after-before) < 0): + raise ValueError('preparation_witness_path_changed') + bins, tail = {}, [] + lower, upper = profile.command.minimum_values[channel], profile.command.maximum_values[channel] + admitted_stamps = {r['image_stamp_ns'] for r in rows} + for row in raw_rows: + if row['image_stamp_ns'] not in admitted_stamps: + tail.clear() + continue + index = row['reference_path_index'] + before = donor.approach_commands[index-1][channel] + after = (donor.approach_commands[index] if index < len(donor.approach_commands) else donor.command)[channel] + value = row['command_vector'][channel] + if not min(before, after) <= value <= max(before, after): + raise ValueError('preparation_witness_path_changed') + try: + bridge_frames((row,), MotionRelation(source.donor, spec.parent_role, spec.child_role)) + except (KeyError, ValueError, TypeError): + tail.clear() + continue + bins[(index, round(128*(value-lower)/(upper-lower)))] = row + if index == len(donor.approach_commands) and value == donor.command[channel]: + tail = (tail+[row])[-11:] + else: + tail.clear() + if len(tail) < 10 or tail[-1]['image_stamp_ns']-tail[0]['image_stamp_ns'] < 200_000_000: + raise ValueError('preparation_witness_stationary_endpoint_missing') + if np.ptp([r['feedback_vector'][channel] for r in tail]) > tolerance: + raise ValueError('preparation_witness_endpoint_unstable') + selected = {r['image_stamp_ns']: r for r in (*bins.values(), *tail)} + return tuple(selected[k] for k in sorted(selected)), tuple(tail) + + +def build_preparation_witness(profile, source, *, legacy=False): + """Replayable source authorization and fresh same-posture verification.""" + spec = profile.measurement.measurements[source.joint] + admission = {} + selected, old_endpoints = select_witness_rows(profile, source, legacy=legacy, admission=admission) + minimum = max(10, profile.acquisition.fixed_reference_minimum_frames) + endpoints = source.endpoint_rows[-minimum:] + if (len(endpoints) < minimum or max(r['image_stamp_ns'] for r in source.rows) >= endpoints[0]['image_stamp_ns'] + or endpoints[-1]['image_stamp_ns']-endpoints[0]['image_stamp_ns'] < 200_000_000 + or [r['image_stamp_ns'] for r in endpoints] != sorted({r['image_stamp_ns'] for r in endpoints})): + raise ValueError('preparation_witness_current_endpoint_missing') + target = profile.motion.joint_zero_references[source.joint] + for row in endpoints: + if (row.get('sample_phase') != 'zero_approach' or row.get('cycle') == 3 + or row.get('input_is_rectified') is not True or row.get('view') != spec.view + or row.get('task_name') != target.task_key or source.joint not in row.get('zero_joints', ()) + or any(row.get(k) != endpoints[0].get(k) for k in ('session_epoch', 'motion_version', 'zero_joints')) + or row.get('command_vector') is None or row.get('feedback_vector') is None + or any(row['command_vector'][i] != target.command[i] for i in source.channels)): + raise ValueError('preparation_witness_current_posture_changed') + old_feedback = np.asarray([r['feedback_vector'] for r in old_endpoints])[:, source.channels] + current_feedback = np.asarray([r['feedback_vector'] for r in endpoints])[:, source.channels] + from .revisit import revisit_feedback_limits + limits = revisit_feedback_limits(profile, 'staged_revisit_v2') + if (not np.all(np.isfinite(current_feedback)) or np.any(np.ptp(current_feedback, axis=0) > limits['within_hold_range']) + or np.any(np.abs(current_feedback-np.median(old_feedback, axis=0)) > limits['between_visit_difference'])): + raise ValueError('preparation_witness_reference_feedback_changed') + relation = MotionRelation(source.donor, spec.parent_role, spec.child_role) + frames = bridge_frames(selected, relation) + current_frames = bridge_frames(endpoints, MotionRelation(source.joint, spec.parent_role, spec.child_role)) + if any(f.camera_matrix != frames[0].camera_matrix or f.reference_frames != frames[0].reference_frames + for f in current_frames): + raise ValueError('preparation_witness_camera_reference_changed') + record = dict(version='preparation_witness_v1' if legacy else 'preparation_witness_v2', + joint=source.joint, donor=source.donor, channels=list(source.channels), + source_frame_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in source.rows}, + endpoint_frame_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in endpoints}, + selected_image_stamps=[r['image_stamp_ns'] for r in selected]) + if not legacy: + record['sample_admission'] = admission + if _insufficient_rotation_before_fit(frames, relation): + record.update(decision='independent_before_fit', reason='insufficient_rotation_evidence') + return None, record + result = resolve_image_motion(frames, (relation,), parameters=ImageMotionParameters(report_pose_uncertainty=True)) + if not result.resolved: + raise ValueError('preparation_witness_unobservable:'+result.reason) + winner = result.hypotheses[result.selected_hypothesis] + bounds = winner.preparation_pose_uncertainty + if len(bounds) != 2 or bounds[0] > math.radians(1) or bounds[1] > .001: + raise ValueError('preparation_witness_pose_uncertain') + source_endpoints = tuple(r for r in old_endpoints if r['image_stamp_ns'] in result.model.training_stamps_ns) + if not source_endpoints: + raise ValueError('preparation_witness_training_endpoint_missing') + poses = [] + for frame in bridge_frames(source_endpoints, relation): + checked = select_image_motion_frame(result.model, frame) + if not checked.resolved: + raise ValueError('preparation_witness_endpoint_unresolved') + by_role = {s.role: s.pose for s in checked.selections} + poses.append(_relative_pose(by_role[spec.parent_role], by_role[spec.child_role])) + rotations = Rotation.from_quat([r.as_quat() for r, _ in poses]) + points = np.asarray([t for _, t in poses]) + if (np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1) + or np.max(np.linalg.norm(points-np.median(points, axis=0), axis=1)) > .001): + raise ValueError('preparation_witness_source_pose_unstable') + current_frames = bridge_frames(endpoints, MotionRelation(source.joint, spec.parent_role, spec.child_role)) + if any(f.camera_matrix != result.model.camera_matrix or f.reference_frames != result.model.reference_frames + for f in current_frames): + raise ValueError('preparation_witness_camera_reference_changed') + bridge = SharedTagPoseBridge(MotionRelation(source.joint, spec.parent_role, spec.child_role), + tuple(rotations.mean().as_quat()), tuple(np.median(points, axis=0)), current_frames, + rotation_uncertainty_rad=bounds[0], translation_uncertainty_m=bounds[1]) + # Independently validate the source on the *new* endpoint too. The + # recipient will still fit every family and keep its own arc holdout. + from ..core.geometry.tag_pose.pose_bridge import check_pose_bridge + from dataclasses import replace + if check_pose_bridge(result.model, replace(bridge, relation=relation)).status != 'consistent': + raise ValueError('preparation_witness_source_pose_changed') + record.update( + source_model=image_model_payload(result.model), source_hypotheses=[asdict(h) for h in result.hypotheses], + selected_hypothesis=result.selected_hypothesis, pose_uncertainty=list(bounds), + quaternion_xyzw=list(bridge.quaternion_xyzw), translation_xyz_m=list(bridge.translation_xyz_m)) + from .branch_initialization import _finite_report + return bridge, _finite_report(record) + + +def _insufficient_rotation_before_fit(frames, relation): + """Decline only when every raw candidate fails the existing motion-span gate. + + No model is fitted or holdout loss inspected. Once auxiliary fitting starts, + its failure must veto; there is no second model choice based on its result. + """ + from itertools import product + from ..core.geometry.tag_pose.motion_image_diagnostics import _validate_frames, _pnp_diagnostics + paths, roles = _validate_frames(frames, (relation,), ImageMotionParameters()) + for indices in product(*(range(len(paths[role])) for role in roles)): + poses = {role: paths[role][index] for role, index in zip(roles, indices)} + if any(pose is None for path in poses.values() for pose in path): + return False # Missing candidates do not prove insufficient motion. + try: + _pnp_diagnostics(frames, (relation,), poses) + except ValueError as error: + if str(error) != 'insufficient_rotation_evidence': + return False + else: + return False + return True diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness_evidence.py new file mode 100644 index 0000000..4e98436 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/preparation_witness_evidence.py @@ -0,0 +1,126 @@ +"""The live witness admission is also the publication and resume audit boundary.""" +from .preparation_witness import WITNESS_POLICY, INDEPENDENT_WITNESS_POLICY, WitnessInput, build_preparation_witness +from .motion_provenance import source_frame_sha256 + + +def check_witness_report(row): + records = row.get('preparation_witnesses', ()) + independent = row.get('image_model_selection_policy') == INDEPENDENT_WITNESS_POLICY + if bool(records) != (row.get('image_model_selection_policy') in {WITNESS_POLICY, INDEPENDENT_WITNESS_POLICY}): + raise ValueError('preparation_witness_selection_policy_changed') + if not records: + return + if any(row.get(k) for k in ('shared_tag_pose_bridges', 'observer_pose_bridges', 'preparation_transitions')): + raise ValueError('preparation_witness_composition_changed') + selected = row.get('selected_hypothesis') + hypotheses = row.get('hypotheses', ()) + if type(selected) is not int or not 0 <= selected < len(hypotheses): + raise ValueError('preparation_witness_winner_missing') + winner = hypotheses[selected] + if independent: + if (any(r.get('version') != 'preparation_witness_v2' or r.get('decision') != 'independent_before_fit' + or r.get('reason') != 'insufficient_rotation_evidence' for r in records) + or winner.get('pose_bridge_checks')): + raise ValueError('preparation_witness_independent_decision_changed') + if (not winner.get('training_accepted') or not winner.get('converged') or winner.get('reason') + or tuple(map(tuple, winner.get('branches', ()))) != tuple(map(tuple, row['frozen_image_model']['branches'])) + or winner.get('child_frame_uncertainty') != row.get('geometry_uncertainty') + or (not independent and not winner.get('pose_bridge_checks')) + or any(c['status'] != 'consistent' for c in winner['pose_bridge_checks'])): + raise ValueError('preparation_witness_winner_changed') + + +def witness_inputs(records, evidence): + observations = {(r['view'], str(r['image_stamp_ns'])): r for r in records + if r.get('kind') == 'motion_branch_observation'} + checked = set() + for payload in evidence.values(): + for record in payload.get('preparation_witnesses', ()): + key = payload['image_model_sha256'], source_frame_sha256(record) + if key in checked: + continue + checked.add(key) + if record.get('version') not in {'preparation_witness_v1', 'preparation_witness_v2'} or record.get('joint') not in payload['zero_joints']: + raise ValueError('preparation_witness_binding_changed') + groups = [] + for field in ('source_frame_hashes', 'endpoint_frame_hashes'): + rows = [] + for stamp, digest in record.get(field, {}).items(): + row = observations.get((payload['view'], stamp)) + if row is None or source_frame_sha256(row) != digest: + raise ValueError('preparation_witness_source_pixels_changed') + rows.append(row) + rows.sort(key=lambda r: r['image_stamp_ns']) + if not rows: + raise ValueError('preparation_witness_source_missing') + groups.append(tuple(rows)) + sources, endpoints = groups + first = sources[0] + visit = [r for r in observations.values() + if all(r.get(k) == first.get(k) for k in ('view', 'task_name', 'zero_joints', 'session_epoch')) + and r['image_stamp_ns'] < min(payload['source_image_stamps']) + and r.get('preparation_witness_joint') == record['donor']] + entered = max((r['image_stamp_ns'] for r in visit if r.get('reference_path_index') == 0), default=0) + complete = [r for r in visit if r.get('reference_path_index', 0) >= 1 and r['image_stamp_ns'] > entered] + if {r['image_stamp_ns'] for r in complete} != {r['image_stamp_ns'] for r in sources}: + raise ValueError('preparation_witness_window_incomplete') + if any(any(r.get(k) != payload.get(k) for k in ('view', 'task_name', 'zero_joints', 'session_epoch', 'motion_version')) + or r['image_stamp_ns'] in payload['source_image_stamps'] + or r['image_stamp_ns'] > max(payload['source_image_stamps']) for r in endpoints): + raise ValueError('preparation_witness_endpoint_binding_changed') + yield payload, record, WitnessInput(record['joint'], record['donor'], sources, endpoints, + tuple(record['channels'])) + + +def validate_witness_topology(profile, records, evidence, source_model): + from ..profiles.preparation import validate_preparation_witnesses + from ..profiles.observations import compile_tag_feedback_channels + validate_preparation_witnesses(profile, source_model) + channels = compile_tag_feedback_channels(profile, source_model) + for _, record, source in witness_inputs(records, evidence): + spec = profile.measurement.measurements[source.joint] + if (profile.motion.preparation_witnesses.get(source.joint) is None + or profile.motion.preparation_witnesses[source.joint].joint != source.donor + or tuple(sorted(channels[spec.parent_role] | channels[spec.child_role])) != source.channels): + raise ValueError('preparation_witness_topology_changed') + + +def validate_preparation_witnesses(profile, records, evidence): + from ..core.geometry.tag_pose.pose_bridge import check_pose_bridge + from ..core.geometry.tag_pose.production_image_motion import resolve_image_motion + from ..core.geometry.tag_pose.image_motion_model import image_model_payload + from ..core.geometry.image_motion_replay import frozen_image_model + from .shared_tag_reference import bridge_frames + observations = {(r['view'], r['image_stamp_ns']): r for r in records + if r.get('kind') == 'motion_branch_observation'} + for payload in evidence.values(): + expected = {joint for joint in payload['zero_joints'] if joint in profile.motion.preparation_witnesses + and profile.measurement.measurements[joint].view == payload['view']} + declared = [r.get('joint') for r in payload.get('preparation_witnesses', ())] + if set(declared) != expected or len(declared) != len(expected): + raise ValueError('preparation_witness_required_evidence_missing') + for payload, record, source in witness_inputs(records, evidence): + if (profile.motion.preparation_witnesses.get(source.joint) is None + or profile.motion.preparation_witnesses[source.joint].joint != source.donor): + raise ValueError('preparation_witness_dependency_changed') + bridge, reproduced = build_preparation_witness(profile, source, + legacy=record['version'] == 'preparation_witness_v1') + if record['version'] == 'preparation_witness_v1': + for old, new in zip(record.get('source_hypotheses', ()), reproduced.get('source_hypotheses', ())): + if 'training_solver_attempts' not in old: + new.pop('training_solver_attempts', None) + if source_frame_sha256(reproduced) != source_frame_sha256(record): + raise ValueError('preparation_witness_replay_changed') + model = frozen_image_model(payload) + if bridge is not None and check_pose_bridge(model, bridge).status != 'consistent': + raise ValueError('preparation_witness_current_model_inconsistent') + # Replay the recipient as well: an edited plausible model must not be + # published merely because its endpoint happens to match the witness. + frames = bridge_frames(tuple(observations[payload['view'], stamp] + for stamp in payload['source_image_stamps']), model.relations[0]) + from ..core.geometry.tag_pose.production_image_motion import image_parameters_for_sampling + result = resolve_image_motion(frames, model.relations, pose_bridges=() if bridge is None else (bridge,), + parameters=image_parameters_for_sampling(payload.get('image_sampling_policy'))) + if (not result.resolved + or source_frame_sha256(image_model_payload(result.model)) != payload['image_model_sha256']): + raise ValueError('preparation_witness_frozen_model_changed') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/profile_contract.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/profile_contract.py new file mode 100644 index 0000000..97505dd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/profile_contract.py @@ -0,0 +1,50 @@ +"""Transfer session profile overrides and verify them before creating motion IO.""" + +from dataclasses import asdict, replace +import json +import math + +from ..core.domain.capture_plan import CapturePlan, configure_training +from ..core.domain.profile import ModelPreparation + + +def profile_launch_parameters(profile): + return { + "training_policy": profile.quality.training_policy, + "preparation_witnesses_json": json.dumps( + {name: asdict(value) for name, value in profile.motion.preparation_witnesses.items()}, + sort_keys=True, separators=(",", ":"), allow_nan=False), + "capture_plan_expected_sha256": CapturePlan.from_profile(profile).sha256, + } + + +def resolve_runtime_profile(profile, value): + """Legacy direct launches retain YAML values; runners send explicit overrides.""" + profile = configure_training(profile, value("training_policy")) + encoded = value("preparation_witnesses_json") + if encoded: + try: + payload = json.loads(encoded) + if not isinstance(payload, dict): + raise ValueError("expected an object") + witnesses = {} + for name, item in payload.items(): + if not isinstance(item, dict) or set(item) != {"joint", "command", "approach_commands"}: + raise ValueError("invalid preparation fields") + if not isinstance(item["joint"], str) or not isinstance(item["approach_commands"], list): + raise ValueError("invalid preparation identity or path") + poses = [item["command"], *item["approach_commands"]] + for pose in poses: + if (not isinstance(pose, list) or len(pose) != profile.command.command_count + or any(type(number) not in (int, float) or not math.isfinite(number) + for number in pose)): + raise ValueError("invalid preparation command") + witnesses[name] = ModelPreparation(item["joint"], tuple(item["command"]), + tuple(tuple(pose) for pose in item["approach_commands"])) + except (TypeError, ValueError) as error: + raise ValueError("invalid preparation_witnesses_json") from error + profile = replace(profile, motion=replace(profile.motion, preparation_witnesses=witnesses)) + expected = value("capture_plan_expected_sha256") + if expected and CapturePlan.from_profile(profile).sha256 != expected: + raise ValueError("runtime_capture_plan_mismatch: session overrides differ from runner") + return profile diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py index 6b1383f..a5d4d85 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -11,6 +11,16 @@ def reason_zh( ) -> tuple[str, str, str]: """Map a common runtime reason to a stable code, explanation and action.""" reason = str(status.get("reason", "unknown")) + if 'shared_pose_handoff_' in reason: + return ('OBS-HANDOFF-120', f'共享 Tag 的任务交接证据不完整或已发生变化：{reason}', + '核对上游回基准的完整原始图像窗口、反馈及时间顺序；交接不能借用验证轮、跨会话旧图或挑选低误差帧。原零位和已通过数据保持冻结。') + if "preparation_sync_" in reason or "preparation_state_vector_invalid" in reason: + return ("OBS-PREP-SYNC-119", "准备观测的指令/反馈同步证据不足或格式无效，不能确认完整运动过程。", + "查看原始帧和 sample_admission 记录中的缺失比例、间隙及端点；不要将此错误直接归因于 Tag 安装，也不放宽同步门限或填造指令。") + if any(code in reason for code in ("image_motion_solver_budget_exhausted", + "image_motion_optimizer_unresolved", "image_motion_solver_timeout")): + return ("OBS-SOLVER-118", "准备几何求解未能在计算预算内收敛，尚不能完成候选评估。", + "查看 motion_branch_initialization 中各候选的求解次数、终止状态与原始图像；先离线分析计算问题，不因这条错误反复运动或调整 Tag。") if reason.startswith("training_quality_failed"): return ("FIT-TRAINING-511", "第三轮训练后质量仍不合格，本任务已结束，未进入独立验证或发布。", "查看 training_decision 的重复性、几何依赖和零位置信区间；保留原始日志，先分析原因。") @@ -78,6 +88,7 @@ def reason_zh( "calibration_node_process_exited": "标定状态发布进程已退出，不能继续本次采集。", "calibration_stack_exited": "标定 ROS 进程栈已退出。", "calibration_node_initial_status_timeout": "启动后未收到标定节点状态；这不是机械堵转诊断。", + "calibration_checkpoint_preparation_timeout": "断点证据校验超过独立加载时限，尚未开始标定运动。", "calibration_device_not_ready": "设备尚未满足启动条件，未自动开始任务运动。", "calibration_start_service_unavailable": "设备报告就绪，但标定开始服务不可用。", "calibration_start_response_timeout": "开始请求未收到确认，不能判断任务是否已启动；运行栈将退出。", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/revisit.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/revisit.py new file mode 100644 index 0000000..19a2817 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/revisit.py @@ -0,0 +1,66 @@ +"""Fresh evidence checks that preserve the original zero and installation.""" + +import numpy as np + +from .joint_resume import reference_pose_failures +from .motion_execution import feedback_tolerances + + +LEGACY_REVISIT_POLICY = "staged_revisit_v1" +REVISIT_POLICY = "staged_revisit_v2" + + +def revisit_feedback_limits(profile, verification=REVISIT_POLICY): + """Separate within-hold stability from between-visit repeatability. + + O30's operator-reported repeatability is 10 native SDK units. This does + not change motion settling, image precision, or another hand's limits. + Historical records retain their original feedback acceptance semantics. + """ + if verification not in {LEGACY_REVISIT_POLICY, REVISIT_POLICY}: + raise ValueError("staged_revisit_policy_unknown") + stability = feedback_tolerances(profile.command.unit).stability + repeatability = 2*stability + if verification == REVISIT_POLICY and profile.key.model.upper() == "O30" and profile.command.unit == "u8": + repeatability = 10.0 + return dict(unit=profile.command.unit, within_hold_range=stability, + between_visit_difference=repeatability) + + +def verify_revisit(profile, previous, current, *, verification=LEGACY_REVISIT_POLICY, + feedback_limits=None): + limits = revisit_feedback_limits(profile, verification) + if ((verification == REVISIT_POLICY and feedback_limits != limits) + or (verification == LEGACY_REVISIT_POLICY and feedback_limits is not None)): + raise ValueError("staged_revisit_feedback_policy_changed") + failures = reference_pose_failures(previous, current) + identities = lambda ref: {(b.view, b.tag_role, b.motion_evidence_id) for b in ref.branch_references} + if not previous.branch_references or identities(previous) != identities(current): + failures.append("installation_evidence_changed") + for view in {s.view for s in previous.samples}: + old = [s for s in previous.samples if s.view == view] + new = [s for s in current.samples if s.view == view] + if not new or min(s.image_stamp_ns for s in new) <= max(s.image_stamp_ns for s in old): + failures.append("fresh_images_missing") + continue + if not profile.vision_motion: + before, after = np.asarray([s.feedback for s in old]), np.asarray([s.feedback for s in new]) + if (before.shape != (len(old), profile.command.command_count) + or after.shape != (len(new), profile.command.command_count) + or not np.all(np.isfinite(before)) or not np.all(np.isfinite(after))): + failures.append("feedback_missing") + else: + if (np.any(np.ptp(after, axis=0) > limits["within_hold_range"]) + or np.any(np.abs(after-np.median(before, axis=0)) > limits["between_visit_difference"])): + failures.append("held_feedback_changed") + if failures: + raise ValueError(f"revisit_reference_changed:{previous.joint}:{','.join(failures)}") + + +def revisit_record(profile, previous, current, cycle): + limits = revisit_feedback_limits(profile) + verify_revisit(profile, previous, current, verification=REVISIT_POLICY, feedback_limits=limits) + return {"kind": "joint_zero_motion_reference_verified", "verification": REVISIT_POLICY, + "feedback_limits": limits, + "joint": previous.joint, "task_name": previous.task_key, "cycle": cycle, + "current_reference": current.as_record(), "preserved_reference": previous.as_record()} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py index 196849e..4c94422 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py @@ -16,6 +16,7 @@ from ..coordinator import CalibrationCoordinator from ..branch_worker import IsolatedBranchSolver from ...core.geometry.tag_pose.image_projection_worker import ImageProjectionWorker from ..inputs import RuntimePorts +from ..profile_contract import resolve_runtime_profile from .io import RosCalibrationIO, _stamp_ns, camera_input, checked_detection_input from .parameters import load_runtime_parameters, parameter_defaults @@ -25,8 +26,7 @@ class UnifiedCalibrationNode(Node): super().__init__(f"{profile.key.model.lower()}_calibration") for name, default in parameter_defaults(profile).items(): self.declare_parameter(name, default) - from ...core.domain.capture_plan import configure_training - profile = configure_training(profile, self.get_parameter("training_policy").value) + profile = resolve_runtime_profile(profile, lambda name: self.get_parameter(name).value) parameters = load_runtime_parameters(profile, lambda name: self.get_parameter(name).value) self.io = RosCalibrationIO(self, profile, parameters) from ..timing_diagnostics import RuntimeTimingDiagnostics @@ -49,9 +49,12 @@ class UnifiedCalibrationNode(Node): self.branch_solver = IsolatedBranchSolver(parameters.session_dir / "motion_solver_diagnostic.jsonl") self.projection_worker = ImageProjectionWorker() from ..artifacts.controller import FinalizationController - self.coordinator = CalibrationCoordinator(profile, parameters, ports, adapter_factory, - finalization=FinalizationController(profile, parameters.session_dir, isolated=True), - initialization_solver=self.branch_solver, projection_worker=self.projection_worker) + from ..startup import checkpoint_heartbeat + with checkpoint_heartbeat(self.io.publish_status, + enabled=parameters.resume_raw_samples_path is not None): + self.coordinator = CalibrationCoordinator(profile, parameters, ports, adapter_factory, + finalization=FinalizationController(profile, parameters.session_dir, isolated=True), + initialization_solver=self.branch_solver, projection_worker=self.projection_worker) from ..detection_dispatch import DetectionDispatcher self.detection_dispatch = DetectionDispatcher(self.coordinator.required_observation_views, self.coordinator.receive_detections, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py index ed5fc82..cead83c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py @@ -30,6 +30,8 @@ def parameter_defaults(profile) -> dict[str, Any]: "resume_raw_samples_path": "", "resume_mode": "verify", "training_policy": profile.quality.training_policy, + "preparation_witnesses_json": "", + "capture_plan_expected_sha256": "", "initial_command_file": "", "command_topic": f"/{model}/cb_right_hand_control_cmd", "state_topic": f"/{model}/cb_right_hand_state", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 8d512f6..8873746 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -30,6 +30,7 @@ def drive_session( ok: Callable[[], bool], request_start: Callable[[], Any], clock: Callable[[], float] = time.monotonic, auto_start: bool = True, startup_timeout: float = 120.0, service_timeout: float = 10.0, + checkpoint_timeout: float = 900.0, ) -> dict[str, Any]: """Send Start once; never infer an SDK fault from console/status age. @@ -37,6 +38,8 @@ def drive_session( discovery have runner deadlines; hardware safety belongs to the node. """ launched = clock() + checkpoint_started = None + device_wait_started = launched ready_since: float | None = None requested_at: float | None = None future = None @@ -60,6 +63,13 @@ def drive_session( return failed("calibration_node_process_exited") if not latest and now - launched > startup_timeout: return failed("calibration_node_initial_status_timeout") + if state == "PREPARING_CHECKPOINT" and requested_at is None: + if checkpoint_started is None: + checkpoint_started = now + if now - checkpoint_started > max(checkpoint_timeout, startup_timeout): + return failed("calibration_checkpoint_preparation_timeout") + device_wait_started = now + continue if state == "READY" and auto_start and requested_at is None: if ready_since is None: @@ -70,7 +80,7 @@ def drive_session( elif now - ready_since > service_timeout: return failed("calibration_start_service_unavailable") elif ready_since is None and requested_at is None: - if auto_start and now - launched > startup_timeout: + if auto_start and now - device_wait_started > startup_timeout: return failed("calibration_device_not_ready:" + str(latest.get("reason", state))) if future is not None and not start_acknowledged: @@ -173,7 +183,8 @@ def run_online( raise RuntimeError(f"existing_calibration_publishers:{occupied}") print(f"{config.serial_number} 标定环境正在启动；日志：{log_path}", flush=True) if resume is not None: - if getattr(config, "resume_mode", "verify") == "passed": + if (getattr(config, "resume_mode", "verify") == "passed" + and profile.quality.training_policy != "staged_2_to_3"): print(f"发现断点：{resume.name}；按已通过任务续采，跳过历史回放和已完成关节零位复核。", flush=True) else: print(f"发现候选断点：{resume.name}；尚未复用，启动后验证基准和 Tag 安装关系。", flush=True) @@ -287,7 +298,7 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--record-bag", action="store_true") parser.add_argument("--commands-disabled", action="store_true") parser.add_argument("--no-resume", action="store_true") - parser.add_argument("--training-policy", choices=("fixed", "adaptive_2_to_3"), default=None, + parser.add_argument("--training-policy", choices=("fixed", "adaptive_2_to_3", "staged_2_to_3"), default=None, help="试验采集策略：两轮训练，必要时补第三轮；保留独立验证，默认沿用配置") parser.add_argument("--startup-timeout-seconds", type=float, default=None, help="等待节点初始化及设备就绪的秒数（默认 120）；大断点逐帧校验可显式延长，不改变运动保护") @@ -302,6 +313,8 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--offline-raw", default="") parser.add_argument("--offline-output", default="") parser.add_argument("--publish-offline", action="store_true") + parser.add_argument("--preparation-policy", choices=("occlusion_aware_witness_v1",), + help="新 O30 staged 会话逐指避让后执行有界侧摆准备；需实机验证，不兼容旧计划断点") selected = parser.parse_args(args) if selected.startup_timeout_seconds is not None and ( not math.isfinite(selected.startup_timeout_seconds) or selected.startup_timeout_seconds <= 0 @@ -328,6 +341,15 @@ def main(args: list[str] | None = None) -> None: parser.error(str(error)) diagnostic_capture = ("o6_thumb" if selected.diagnostic_thumb else "o6_yaw_hold" if selected.diagnostic_yaw_hold else selected.diagnostic_capture) + if selected.preparation_policy is not None: + from dataclasses import replace + from ..product import ProductCalibrationContract + from ..profiles.preparation import configure_preparation + try: + profile = configure_preparation(config.calibration_contract.typed_profile, selected.preparation_policy) + config = replace(config, calibration_contract=ProductCalibrationContract(declarative=profile)) + except ValueError as error: + parser.error(str(error)) diagnostic = None if diagnostic_capture: if config.calibration_contract.typed_profile.quality.training_policy != "fixed": diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py index c5ce682..410e48c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py @@ -20,6 +20,7 @@ from std_srvs.srv import Trigger from ..operator_report import ProgressEstimator from ..product import ProductConfig +from .profile_contract import profile_launch_parameters class ProgressConsole: @@ -126,7 +127,7 @@ def launch_command( "record_bag": str(record_bag).lower(), "diagnostic_capture": diagnostic_capture, "resume_mode": getattr(config, "resume_mode", "verify"), - "training_policy": config.calibration_contract.typed_profile.quality.training_policy, + **profile_launch_parameters(config.calibration_contract.typed_profile), } startup_speed = sdk_startup_speed_u8 if startup_speed is None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py index 2a2373c..5a16c6c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py @@ -25,6 +25,8 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, Reference travel is learned only from an accepted first training cycle. """ engine = CalibrationEngine(profile) + if hasattr(records, "task_records"): + records = records.task_records(unit.task_key) task = next(task for task in profile.motion.tasks if task.key == unit.task_key) segment = task_segment(task, unit.segment_key) if unit.segment_key else None streams = [s for s in observation_streams(profile, task) if segment is None or s[1] in segment.joints] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py index c80f2ce..b158e04 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -89,6 +89,10 @@ class CalibrationSession: if current is not None: self._unit_index = next(i for i, unit in enumerate(self._units) if unit.identity == current.identity) + def supplement_training(self, task_keys): + for task_key in task_keys: + self.select_training(task_key, (0, 1, 2)) + def device_ready(self) -> None: self._require(CalibrationPhase.WAIT_DEVICE) self.phase = CalibrationPhase.READY diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_pose_handoff.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_pose_handoff.py new file mode 100644 index 0000000..3063ae5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_pose_handoff.py @@ -0,0 +1,115 @@ +"""Two-sided, immutable evidence for handing a shared Tag to another joint. + +The source is checked on its last baseline images BEFORE recipient motion. +The recipient is checked on its own return images AFTER that motion. Neither +window is selected by pixel loss, and neither model nor zero is rewritten. +""" +from collections import deque + +import numpy as np + +from ..core.geometry.tag_pose.ippe import solve_square_tag_ippe +from ..core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates +from ..core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from .motion_execution import feedback_tolerances + +HANDOFF_POLICY = 'two_sided_shared_pose_v1' +SOURCE_PHASES = frozenset({'sweep', 'steady', 'joint_zero'}) + + +def enabled(profile): + return (profile.quality.training_policy == 'staged_2_to_3' + and bool(profile.motion.preparation_witnesses)) + + +class SharedPoseHandoff: + """Bounded raw-image tails, owned by the existing reference registry.""" + + def __init__(self, profile): + self.profile = profile + self.windows = {} + + def observe(self, row): + if not enabled(self.profile) or row.get('kind') != 'image_observation_frame': + return + if row.get('sample_phase') not in SOURCE_PHASES: + return + key = (row.get('session_epoch'), row.get('task_name'), row.get('view')) + window = self.windows.setdefault(key, deque(maxlen=max(10, + self.profile.acquisition.fixed_reference_minimum_frames))) + if window and window[-1]['session_epoch'] != row['session_epoch']: + window.clear() + if not window or row['image_stamp_ns'] > window[-1]['image_stamp_ns']: + window.append(row) + + def rows(self, epoch, reference, view): + return tuple(self.windows.get((epoch, reference.task_key, view), ())) + + def advance_recovery_epoch(self, previous, current): + # Called only by the registry's journalled internal recovery. Raw rows + # keep their original epoch; a restart cannot copy this in-memory grant. + self.windows.update({(current, task, view): deque(rows, maxlen=rows.maxlen) + for (epoch, task, view), rows in tuple(self.windows.items()) if epoch == previous}) + + +def source_window(profile, rows, reference, channels, model, *, epoch, before_stamp): + """Validate the entire predetermined tail, then reconstruct all IPPE seeds. + + A bad/missing frame vetoes this window. Current-image root verification, + the 1.5 px gate and the source-zero pose bounds still run in check_source_pose. + """ + minimum = max(10, profile.acquisition.fixed_reference_minimum_frames) + if len(rows) != minimum: + raise ValueError('shared_pose_handoff_source_missing') + stamps = [r['image_stamp_ns'] for r in rows] + spec = profile.measurement.measurements[reference.joint] + old = [s for s in reference.samples if s.view == spec.view] + if (not old or stamps != sorted(set(stamps)) or stamps[-1] >= before_stamp + or stamps[0] < min(s.image_stamp_ns for s in old) + or not 200_000_000 <= stamps[-1]-stamps[0] + <= profile.acquisition.joint_zero_timeout_seconds*1e9): + raise ValueError('shared_pose_handoff_window_invalid') + target = profile.motion.joint_zero_references[reference.joint].command + for row in rows: + if (row.get('kind') != 'image_observation_frame' or row.get('session_epoch') != epoch + or row.get('task_name') != reference.task_key or row.get('view') != spec.view + or row.get('sample_phase') not in SOURCE_PHASES + or row.get('cycle') == profile.quality.holdout_cycle + or row.get('command_unit') != profile.command.unit + or row.get('input_is_rectified') is not True + or row.get('camera_matrix_source') != 'CameraInfo.P[:3,:3]' + or tuple(map(tuple, row['camera_matrix'])) != model.camera_matrix): + raise ValueError('shared_pose_handoff_identity_changed') + for field in ('command_vector', 'feedback_vector'): + values = row.get(field) + if (values is None or np.shape(values) != (profile.command.command_count,) + or not np.all(np.isfinite(values))): + raise ValueError('shared_pose_handoff_state_missing') + if any(row['command_vector'][i] != target[i] for i in channels): + raise ValueError('shared_pose_handoff_posture_changed') + feedback = np.asarray([r['feedback_vector'] for r in rows])[:, channels] + previous = np.asarray([s.feedback for s in old])[:, channels] + tolerance = feedback_tolerances(profile.command.unit).stability + if (np.any(np.ptp(feedback, axis=0) > tolerance) + or np.any(np.abs(feedback-np.median(previous, axis=0)) > 2*tolerance)): + raise ValueError('shared_pose_handoff_feedback_changed') + references = {ref.role: ref for ref in model.reference_frames} + if set(references) != {spec.parent_role}: + raise ValueError('shared_pose_handoff_fixed_root_required') + frames = [] + tags = {tag.role:tag for view in profile.vision.views if view.name == spec.view for tag in view.tags} + for row in rows: + roles, observations = [], [] + for role, size in model.tag_sizes: + item = row['tags'].get(role) + if (item is None or item.get('tag_size_m') != size + or item.get('tag_id') != tags[role].tag_id): + raise ValueError('shared_pose_handoff_pixels_missing') + corners = tuple(map(tuple, item['corners_xy'])) + candidates = ((references[role].pose,) if role in references else + tuple(solve_square_tag_ippe(corners, tag_size_m=size, camera_matrix=model.camera_matrix))) + roles.append(RoleCandidates(role, candidates, role in references)) + observations.append(ImageRoleObservation(role, size, corners)) + frames.append(ImageMotionFrame(MotionEvidenceFrame(row['image_stamp_ns'], tuple(roles)), + model.camera_matrix, tuple(observations), model.reference_frames)) + return tuple(frames) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py index 9895318..7d6f3f5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py @@ -9,6 +9,7 @@ from scipy.spatial.transform import Rotation from ..core.geometry.tag_pose.pose_bridge import ( check_pose_bridge, check_source_pose, SHARED_ZERO_CONDITIONING, SHARED_POSE_CONDITIONING, + is_handoff_policy, ) from .parent_reference_evidence import source_epoch_matches from .shared_tag_reference import build_pose_bridge, shared_zero_source @@ -24,6 +25,17 @@ def validate_shared_tag_references(profile, records, evidence, *, source_model=N sources = {source_frame_sha256(r): r for r in source_references} observations = {(r['view'], str(r['image_stamp_ns'])): r for r in records if r.get('kind')=='motion_branch_observation'} + # A resumed scan and its dependency import may both carry the same raw + # exposure. Count it once, and reject conflicting copies of that identity. + raw_images = {} + for row in records: + if row.get('kind') != 'image_observation_frame': + continue + identity = (row.get('session_epoch'), row.get('task_name'), row.get('view'), row['image_stamp_ns']) + previous = raw_images.get(identity) + if previous is not None and previous != row: + raise ValueError('shared_pose_handoff_duplicate_image_changed') + raw_images[identity] = row samples = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind')=='joint_zero_sample'} pixels = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind')=='pnp_candidate_frame'} channels = None if source_model is None else compile_tag_feedback_channels(profile, source_model) @@ -84,6 +96,32 @@ def validate_shared_tag_references(profile, records, evidence, *, source_model=N validate_image_sample(row, frame, replay_image_models(frame)) bridge = build_pose_bridge(profile, joint, reference, rows, tuple(held)) current_model, source_image_model = frozen_image_model(payload), frozen_image_model(source) + if is_handoff_policy(payload.get('image_model_selection_policy')): + from .shared_pose_handoff import HANDOFF_POLICY, SOURCE_PHASES, source_window + from .parent_reference_evidence import same_capture_epoch + if record.get('handoff_policy') != HANDOFF_POLICY: + raise ValueError('shared_pose_handoff_policy_changed') + # Recompute the chronological tail from ALL recorded images; + # hashes of a hand-picked set are not sufficient evidence. + before = min(payload['source_image_stamps']) + available = sorted((r for r in raw_images.values() + if r.get('session_epoch') == record.get('source_window_epoch') + and r.get('task_name') == reference.task_key and r.get('view') == spec.view + and r.get('sample_phase') in SOURCE_PHASES and r['image_stamp_ns'] < before), + key=lambda r:r['image_stamp_ns']) + minimum = max(10, profile.acquisition.fixed_reference_minimum_frames) + window = available[-minimum:] + if record.get('source_window_hashes') != { + str(r['image_stamp_ns']): source_frame_sha256(r) for r in window}: + raise ValueError('shared_pose_handoff_window_changed') + if not window or not same_capture_epoch(records, record.get('source_window_epoch'), + payload['session_epoch'], window[-1]['image_stamp_ns'], before): + raise ValueError('shared_pose_handoff_epoch_changed') + bridge = replace(bridge, source_frames=source_window(profile, window, + reference, tuple(held), source_image_model, + epoch=record['source_window_epoch'], before_stamp=before)) + elif 'handoff_policy' in record or 'source_window_hashes' in record: + raise ValueError('shared_pose_handoff_policy_changed') conditioning = record.get('conditioning') if conditioning is not None: if conditioning not in {SHARED_ZERO_CONDITIONING, SHARED_POSE_CONDITIONING}: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py index 252a1bc..5d33457 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py @@ -135,10 +135,22 @@ def prepare_pose_bridges(owner, view, references, sources, arc_stamps, epoch): rows = tuple(row for row in sources if row['image_stamp_ns'] not in arc_stamps)[-minimum:] bridge = replace(build_pose_bridge(owner.profile, joint, reference, rows, channels), source_model=model.image_model) + from .shared_pose_handoff import enabled, source_window, HANDOFF_POLICY + handoff_record = {} + if enabled(owner.profile): + source_rows = owner.parent_references.handoff.rows(epoch, reference, view) + source_epoch = source_rows[0]['session_epoch'] if source_rows else epoch + source_frames = source_window(owner.profile, source_rows, reference, channels, + model.image_model, epoch=source_epoch, before_stamp=min(arc_stamps)) + bridge = replace(bridge, source_frames=source_frames) + handoff_record = dict(handoff_policy=HANDOFF_POLICY, + source_window_epoch=source_epoch, + source_window_hashes={str(r['image_stamp_ns']): source_frame_sha256(r) for r in source_rows}) if any(f.camera_matrix != model.image_model.camera_matrix for f in bridge.frames): raise ValueError('shared_pose_camera_changed') bridges.append(bridge) records.append(dict(joint=joint, source_reference=reference.as_record(), + **handoff_record, conditioning=bridge.conditioning, source_evidence_id=identity, source_model_sha256=report['image_model_sha256'], held_channels=list(channels), diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_resume.py new file mode 100644 index 0000000..57120cb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_resume.py @@ -0,0 +1,93 @@ +"""Resume complete visits without crossing a frozen training boundary.""" + +from collections import OrderedDict +from pathlib import Path +import json + +from ..core.domain.capture_plan import evidence_digest +from ..core.domain.motion_path import record_scan_identity +from ..core.domain.reference import read_joint_zero_references +from .motion_provenance import validate_motion_provenance +from .scan_quality import evaluate_capture_unit + + +def stage_visits(resume, engine, rows): + references = read_joint_zero_references(resume.profile, rows) + validate_motion_provenance(resume.profile, rows, references=references, + image_replay_workers=resume.image_replay_workers) + visits, latest = OrderedDict(), {} + for unit in engine.scan_units(): + visits.setdefault((unit.task_key, unit.cycle), []).append(unit) + for row in rows: + if row.get('kind') == 'scan_unit_complete': + latest[record_scan_identity(row)] = row + spans, valid = {}, set() + for (key, _), units in visits.items(): + task = next(t for t in resume.profile.motion.tasks if t.key == key) + if not set(task.joints) <= references.keys(): + break + if any(latest.get(u.identity, {}).get('passed') is not True for u in units): + break + if not all(evaluate_capture_unit(resume.profile, u, latest[u.identity].get('attempt', 1), rows, + first_cycle_spans=spans).passed for u in units): + raise ValueError('staged_resume_completed_visit_invalid') + valid.update(u.identity for u in units) + decisions = [r for r in rows if r.get('kind') == 'staged_training_decision'] + if any(r['decision'] == 'fail' for r in decisions): + raise ValueError('staged_failed_training_cannot_resume') + if any(r['decision'] == 'freeze' for r in decisions): + if not {u.identity for u in engine.scan_units() if u.cycle != 3} <= valid: + raise ValueError('staged_frozen_training_prefix_incomplete') + resume.references, resume.rows, resume.units = references, tuple(rows), valid + resume.first_cycle_spans = spans + + +def load_frozen_checkpoint(profile, rows, path, source): + from .staged_training import read_frozen_training, staged_snapshot + freezes = [r for r in rows if r.get('kind') == 'staged_training_decision' and r['decision'] == 'freeze'] + if not freezes: + return None + payload = json.loads((Path(path).parent/'frozen_training.json').read_text()) + if len(freezes) != 1 or evidence_digest(payload) != freezes[0]['frozen_training_sha256']: + raise ValueError('staged_resume_frozen_model_changed') + read_frozen_training(profile, staged_snapshot(profile, rows), read_joint_zero_references(profile, rows), source, payload) + return payload + + +def reference_import(resume): + from .joint_resume import _index_motion_provenance + from .passed_resume import PassedReferenceImport + provenance = tuple(_index_motion_provenance(resume.rows).values()) + models = tuple(r for r in provenance if r.get('kind') == 'motion_branch_initialization' and r.get('status') == 'resolved') + return PassedReferenceImport(rows=(*(r.as_record() for r in resume.references.values()), *provenance), + references=resume.references, model_records=models) + + +def restore_recovery(session, recovery, rows): + for row in rows: + if row.get('kind') == 'joint_zero_recovery': + recovery.restore(row['task_name'], row['joints']) + elif row.get('kind') == 'scan_unit_complete': + if row.get('passed') is False and int(row.get('attempt', 1)) >= 2: + raise ValueError('staged_resume_recovery_exhausted') + key = record_scan_identity(row) + consumed = max(0, int(row.get('attempt', 1))-1) + (row.get('passed') is False) + session._retry_by_unit[key] = max(session._retry_by_unit.get(key, 0), min(1, consumed)) + + +def visit_rows(resume, task_key, cycle): + task = next(t for t in resume.profile.motion.tasks if t.key == task_key) + if not all((name, cycle) in resume.verified_visits for name in task.joints): + return (), () + units = {key for key in resume.units if key[0] == task_key and key[1] == cycle} + rows = [r for r in resume.rows if record_scan_identity(r) in units + and r.get('kind') not in {'joint_zero_reference', 'joint_zero_sample'}] + if units: + from .engine import CalibrationEngine + stage = [u for u in CalibrationEngine(resume.profile).scan_units() if u.cycle == cycle] + if cycle in {1, 2} and stage[-1].task_key == task_key: + decisions = [r for r in resume.rows if r.get('kind') == 'staged_training_decision'] + candidates = decisions[:1] if cycle == 1 else decisions[1:] + rows.extend(candidates) + resume.units.difference_update(units) + return tuple(sorted(units)), tuple(rows) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_training.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_training.py new file mode 100644 index 0000000..b6836b5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/staged_training.py @@ -0,0 +1,275 @@ +"""Whole-hand training checkpoints and one dependency-complete supplement.""" + +from dataclasses import dataclass, replace +import hashlib +import math +from pathlib import Path + +from ..core.domain.capture_plan import CapturePlan, evidence_digest, select_task_training, training_cycles +from ..core.urdf.kinematics import UrdfKinematicModel +from ..core.fitting.command_motion import fit_command_training, CommandMotionFit +from ..core.fitting.frozen import encode, decode +from ..core.fitting.session import compile_spatial_profile, build_axis_observations +from ..core.fitting.spatial_solver.solve import fit_urdf_zero_training +from ..core.fitting.training_quality import _by_joint, _curve_repeatability + + +STAGED_DECISION_VERSION = "staged_training_decision_v1" +FROZEN_VERSION = "frozen_hand_training_v1" +SOLVER_VERSION = "staged_command_spatial_tag_v1" + + +@dataclass(frozen=True) +class TrainingIssue: + code: str + scope: str + parameters: tuple[str, ...] + tasks: tuple[str, ...] + + +def dependency_tasks(profile, parameter): + """Same-cycle observation closure; shared/global failures use all tasks.""" + graph = profile.zero.spatial + if graph.get('base_pose_strategy') == 'full_hand' and parameter in graph.get('root_anchor_joints', ()): + return tuple(t.key for t in profile.motion.tasks) + observer = graph.get("offset_observer_joint", {}).get(parameter) + if observer is None: + return tuple(t.key for t in profile.motion.tasks) + pending, joints = [parameter, observer], set() + while pending: + joint = pending.pop() + if joint in joints: + continue + joints.add(joint) + for key in ("axis_parent_joint", "phase_parent_joint"): + parent = graph.get(key, {}).get(joint) + if parent is not None: + pending.append(parent) + return tuple(t.key for t in profile.motion.tasks if set(t.joints) & joints) + + +def training_input_sha256(profile, rows, references, source_urdf): + return evidence_digest({"plan": CapturePlan.from_profile(profile).as_dict(), "rows": rows, + "configuration": encode(profile), "solver_version": SOLVER_VERSION, + "references": {name: ref.as_record() for name, ref in sorted(references.items())}, + "source_urdf_sha256": hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()}) + + +def spatial_inputs(profile, source_urdf, model, steady, motion, *, include_holdout): + zero_profile = compile_spatial_profile(profile) + axes = build_axis_observations(profile, model, steady, motion, zero_profile, include_holdout=include_holdout) + baselines = {**{n: v.cad_angle_rad for n, v in profile.zero.known_baseline_geometry.items()}, + **{n: 0. for n in profile.zero.cad_zero_assumptions}} + curves = {n: replace(c, circle={**c.circle, "known_cad_baseline_rad": baselines[n]}) + if n in baselines and n not in zero_profile.direct_zero_joints else c + for n, c in motion.coordinate_curves.items()} + from ..core.fitting.motion_fit import channel_for_joint + return dict(source_urdf=source_urdf, measurements=axes, + curves=curves, motor_by_joint={n: channel_for_joint(profile, n) for n in curves}, + zero_profile=zero_profile, + training_cycles=tuple(sorted({c for t in profile.motion.tasks for c in training_cycles(profile, t)})), + allow_partial_training_cycles=True, maximum_offset_rad=math.radians(20), + finger_maximum_offset_rad=math.radians(20), maximum_validation_mae_rad=math.radians(1), + maximum_validation_p95_rad=math.radians(2), maximum_validation_error_rad=math.radians(3), + maximum_confidence_half_width_rad=math.radians(1), maximum_pose_axis_line_rms_m=.0015) + + +def assess_staged_training(*, profile, rows, references, source_urdf, cache=None, **_): + """Only current training rows enter; cache keys include every local input.""" + if any(r["cycle"] not in training_cycles(profile, r["task_name"]) for r in rows): + raise ValueError("staged_training_received_holdout") + cache = {} if cache is None else cache + model = UrdfKinematicModel(source_urdf) + sweep, steady = _by_joint(rows, "sweep"), _by_joint(rows, "steady") + source_hash = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() + parts, issues, metrics = [], [], {} + profile_contract = encode(replace(profile, quality=replace(profile.quality, task_training_cycles={}))) + for task in profile.motion.tasks: + names = task.joints + try: + part_sweep, part_steady = ({n: sweep[n] for n in names}, {n: steady[n] for n in names}) + refs = {n: references[n] for n in names} + key = evidence_digest({"source": source_hash, "task": task.key, + "profile": profile_contract, "cycles": training_cycles(profile, task), + "sweep": part_sweep, "steady": part_steady, + "references": {n: r.as_record() for n, r in refs.items()}}) + cached = cache.get(task.key) + local = cached[1] if cached and cached[0] == key else fit_command_training( + profile, model, part_sweep, part_steady, refs) + cache[task.key] = (key, local) + failures, observed = _curve_repeatability(profile, task, local, steady, training_cycles(profile, task)) + metrics.update(observed) + if failures: + issues.append(TrainingIssue("training_repeatability", "task", tuple(names), (task.key,))) + parts.append(local) + except (ValueError, KeyError) as error: + issues.append(TrainingIssue("local_training_invalid", "task", tuple(names), (task.key,))) + metrics[task.key] = {"reason": str(error)} + motion, zero, frozen = None, None, None + if len(parts) == len(profile.motion.tasks): + motion = CommandMotionFit(**{field: {k: v for part in parts for k, v in getattr(part, field).items()} + for field in CommandMotionFit.__dataclass_fields__}) + try: + zero = fit_urdf_zero_training(**spatial_inputs(profile, source_urdf, model, steady, motion, include_holdout=False)) + metrics['spatial'] = {'zero_confidence_half_width_rad': { + n: v if math.isfinite(v) else None for n, v in zero.confidence_half_widths.items()}, + 'independent_cycles_by_joint': {name: len(values) for name, values in zero.cycle_values.items()}} + for name, code in zero.failure_reasons.items(): + tasks = dependency_tasks(profile, name) + issues.append(TrainingIssue(code, "parameter" if name in profile.zero.direct_zero_joints else "global", + (name,), tasks)) + except (ValueError, KeyError) as error: + issues.append(TrainingIssue("spatial_training_invalid", "global", (), tuple(t.key for t in profile.motion.tasks))) + metrics["spatial_failure"] = str(error) + if not issues and motion is not None and zero is not None: + try: + frozen = _freeze(profile, rows, references, source_urdf, model, motion, zero) + except ValueError as error: + issues.append(TrainingIssue("tag_training_invalid", "global", (), tuple(t.key for t in profile.motion.tasks))) + metrics["tag_failure"] = str(error) + supplemented = any(2 in training_cycles(profile, t) for t in profile.motion.tasks) + decision = "fail" if issues and supplemented else "supplement" if issues else "freeze" + tasks = tuple(t.key for t in profile.motion.tasks if any(t.key in issue.tasks for issue in issues)) + record = {"kind": "staged_training_decision", "version": STAGED_DECISION_VERSION, + "decision": decision, "supplement_tasks": list(tasks) if decision == "supplement" else [], + "issues": [{"code": i.code, "scope": i.scope, "parameters": list(i.parameters), + "tasks": list(i.tasks)} for i in issues], "metrics": metrics, + "failures": [i.code for i in issues], + "source_sha256": training_input_sha256(profile, rows, references, source_urdf), + "input_plan_sha256": CapturePlan.from_profile(profile).sha256, + "frozen_training_sha256": None if frozen is None else evidence_digest(frozen)} + record["decision_sha256"] = evidence_digest(record) + return {"decision": record, "frozen_training": frozen} + + +def _freeze(profile, rows, references, source, model, motion, zero): + from .artifacts.evidence import prepare_tag_replay + from ..core.urdf.plan import StandardUrdfPlan + from ..core.urdf.patch import UrdfPatchSet + from ..core.geometry.extrinsics import transform_matrix + offsets = {**zero.frozen.applied_offsets, **{n: 0. for n in profile.zero.cad_zero_assumptions}} + if any(j.mimic_joint for j in model.joints.values()): + raise ValueError("staged_tag_training_requires_independent_joint_model") + plan = StandardUrdfPlan(hashlib.sha256(Path(source).read_bytes()).hexdigest(), offsets, {}, {}, + UrdfPatchSet({}), profile.urdf_authorized_fields) + pose = zero.frozen.fit + replay = prepare_tag_replay(profile=profile, source_model=model, plan=plan, + output_mappings=motion.mappings, + common_from_base=transform_matrix(pose.base_translation, pose.base_rotation.as_quat()), + records=[r for r in rows if r.get("sample_phase") == "steady"]) + return {"version": FROZEN_VERSION, "solver_version": SOLVER_VERSION, + "input_sha256": training_input_sha256(profile, rows, references, source), + "capture_plan": CapturePlan.from_profile(profile).as_dict(), "motion": encode(motion), + "zero": encode(zero.frozen), "tag_installations": encode(dict(replay.installations)), + "common_from_base": encode(replay.common_from_base), "required_roles": list(replay.required_roles)} + + +def read_frozen_training(profile, rows, references, source, payload): + if (payload.get("version") != FROZEN_VERSION or payload.get("solver_version") != SOLVER_VERSION + or payload.get("capture_plan") != CapturePlan.from_profile(profile).as_dict() + or payload.get("input_sha256") != training_input_sha256(profile, rows, references, source)): + raise ValueError("frozen_hand_training_inputs_changed") + return {key: decode(payload[key]) for key in ("motion", "zero", "tag_installations", "common_from_base")} + + +def staged_snapshot(profile, records): + from .capture_index import task_training_snapshot + if hasattr(records, 'training_task_snapshot'): + return tuple(row for task in profile.motion.tasks for row in + records.training_task_snapshot(task.key, training_cycles(profile, task))) + groups = {} + for row in records: + groups.setdefault(row.get('task_name'), []).append(row) + return tuple(row for task in profile.motion.tasks for row in + task_training_snapshot(groups.get(task.key, ()), training_cycles(profile, task))) + + +def resolve_staged_plan(profile, records, *, source_urdf=None, verify_decisions=False, require_complete=False): + """Replay phase transitions before trusting any partition or frozen file.""" + from ..core.domain.capture_plan import STAGED, configure_training + from ..core.domain.reference import JointZeroReference + from ..core.domain.motion_path import record_scan_identity + from .engine import CalibrationEngine + current = configure_training(profile, STAGED) + headers = [r for r in records if r.get("kind") == "session_start"] + if len(headers) != 1 or headers[0].get("capture_plan") != CapturePlan.from_profile(current).as_dict(): + raise ValueError("staged_capture_requires_matching_plan_header") + phase, supplemented, frozen, failed = 0, False, False, False + completed, prefix, references, revisits = set(), [], {}, {} + units = CalibrationEngine(current).scan_units() + allowed = {u.identity for u in units} + first_round = {u.identity for u in units if u.cycle == 0} + for row in records: + kind = row.get("kind") + if kind == "joint_zero_reference": + reference = JointZeroReference.from_record(row) + old = references.get(reference.joint) + if old is not None and old.as_record() != reference.as_record(): + raise ValueError("staged_frozen_reference_replaced") + references[reference.joint] = reference + if kind == "scan_unit_complete": + identity = record_scan_identity(row) + if row.get("passed"): + completed.add(identity) + else: + completed.discard(identity) + elif kind == "joint_zero_motion_reference_verified" and str(row.get("verification", "")).startswith("staged_revisit"): + from .revisit import verify_revisit + original = JointZeroReference.from_record(row["preserved_reference"]) + current_reference = JointZeroReference.from_record(row["current_reference"]) + if (original.joint not in references or row.get("joint") != original.joint + or row.get("task_name") != original.task_key + or evidence_digest(original.as_record()) != evidence_digest(references[original.joint].as_record())): + raise ValueError("staged_revisit_zero_binding_changed") + verify_revisit(current, original, current_reference, verification=row["verification"], + feedback_limits=row.get("feedback_limits")) + visit = revisits.setdefault((original.task_key, row.get("cycle")), {}) + # Imported scans retain their original, already verified visit. + # A later physical recheck must not erase that historical proof. + visit[original.joint] = min(visit.get(original.joint, math.inf), + max(s.image_stamp_ns for s in current_reference.samples)) + elif kind == "staged_training_decision": + expected = {u.identity for u in units if u.cycle != 3} + if (failed or frozen or not expected <= completed + or row.get("version") != STAGED_DECISION_VERSION + or row.get("input_plan_sha256") != CapturePlan.from_profile(current).sha256 + or row.get("decision_sha256") != evidence_digest({k: v for k, v in row.items() if k != "decision_sha256"})): + raise ValueError("staged_training_decision_invalid") + if verify_decisions: + result = assess_staged_training(profile=current, rows=staged_snapshot(current, prefix), + references=references, source_urdf=source_urdf) + if evidence_digest(result["decision"]) != evidence_digest(row): + raise ValueError("staged_training_decision_evidence_changed") + decision = row.get("decision") + if decision == "supplement" and not supplemented: + keys = row.get("supplement_tasks", []) + if not keys or len(set(keys)) != len(keys): + raise ValueError("staged_supplement_tasks_invalid") + for key in keys: + current = select_task_training(current, key, (0, 1, 2)) + units = CalibrationEngine(current).scan_units() + allowed = {u.identity for u in units} + supplemented, phase = True, 2 + elif decision == "freeze" and row.get("frozen_training_sha256"): + frozen, phase = True, 3 + elif decision == "fail": + failed = True + else: + raise ValueError("staged_training_transition_invalid") + elif kind in {"joint_sample", "secondary_joint_sample"}: + cycle = row.get("cycle") + if cycle == 1 and phase == 0 and first_round <= completed: + phase = 1 + if failed or cycle != phase or record_scan_identity(row) not in allowed: + raise ValueError("staged_observation_outside_current_phase") + task = next(t for t in current.motion.tasks if t.key == row["task_name"]) + if cycle > 0: + visit = revisits.get((task.key, cycle), {}) + if not set(task.joints) <= visit.keys(): + raise ValueError("staged_visit_reference_not_verified") + if row['image_stamp_ns'] <= max(visit[name] for name in task.joints): + raise ValueError("staged_visit_images_precede_reference_check") + prefix.append(row) + if require_complete and (not frozen or failed): + raise ValueError("staged_training_not_frozen") + return current diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/startup.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/startup.py new file mode 100644 index 0000000..d8ffbd9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/startup.py @@ -0,0 +1,37 @@ +"""Publish initialization liveness before the ROS executor can start. + +The publisher owns no coordinator state, SDK ports or start service. Its +lifetime ends before normal status callbacks begin, including on exceptions. +""" +from contextlib import contextmanager +from threading import Event, Thread + +from ..core.domain.status import CalibrationStatus + + +@contextmanager +def checkpoint_heartbeat(publish, *, enabled, interval=1.0): + if not enabled: + yield + return + status = CalibrationStatus(state='PREPARING_CHECKPOINT', phase='PREPARING_CHECKPOINT', + resume={'message': '正在加载并校验断点原始证据，尚未开始标定运动'}) + stopped = Event() + errors = [] + def heartbeat(): + while not stopped.wait(interval): + try: + publish(status) + except Exception as error: + errors.append(error) + return + publish(status) + worker = Thread(target=heartbeat, name='checkpoint-status', daemon=True) + worker.start() + try: + yield + finally: + stopped.set() + worker.join() + if errors: + raise RuntimeError('checkpoint_status_publication_failed') from errors[0] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py index 124c0ef..5f79b9b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -12,6 +12,7 @@ from .reporting.reasons_zh import reason_zh _STATES = {"WAIT_START": "READY", "WAIT_DEVICES": "WAIT_DEVICE", "PASSED": "COMPLETE"} _PHASES = { + "PREPARING_CHECKPOINT": "加载并校验断点证据（未开始运动）", "WAIT_DEVICE": "等待设备", "READY": "设备就绪", "BASELINE": "安全恢复基准", "REFERENCE_LOCKING": "锁定基准（等待所需 Tag）", "RESUME_VERIFY": "验证断点基准", "REFERENCE_POSES": "采集 Tag 安装验证姿态", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py index 7950088..b69dd71 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py @@ -15,7 +15,8 @@ from .engine import SweepQuality def evaluate_task_input_support(profile, task, records): - rows = [row for row in records if row.get("task_name") == task.key] + rows = (records.task_records(task.key) if hasattr(records, "task_records") else + [row for row in records if row.get("task_name") == task.key]) latest = {} for row in rows: if all(key in row for key in ("cycle", "direction")): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py index 0971742..dd32be0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py @@ -15,6 +15,11 @@ class StageTiming: self.stage = "startup" self.seconds = {} self.transitions = {} + self.capture_started = self.terminal_started = None + + def mark_start(self, now): + self.capture_started = float(now) + self.terminal_started = None def enter(self, stage, now): now = float(now) @@ -23,10 +28,15 @@ class StageTiming: if stage != self.stage: self.transitions[stage] = self.transitions.get(stage, 0) + 1 self.stage = stage + if stage == 'terminal' and self.terminal_started is None: + self.terminal_started = now def as_dict(self, now): self.enter(self.stage, now) return {"schema_version": 1, "elapsed_seconds": max(0., float(now) - self.started), + "start_to_terminal_seconds": (None if self.capture_started is None or self.terminal_started is None + else max(0., self.terminal_started-self.capture_started)), + "checkpoint_preload_seconds": self.seconds.get('resume_loading', 0.), "stage_seconds": dict(self.seconds), "stage_entries": dict(self.transitions), "clock": "monotonic", "resolution": "control_tick_boundaries"} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py index d49ade0..4e9ae8d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py @@ -1,12 +1,12 @@ """Causal training-plan replay and isolated live assessment.""" -from concurrent.futures import ThreadPoolExecutor +from concurrent.futures import Future, ThreadPoolExecutor import multiprocessing import threading import time import traceback -from ..core.domain.capture_plan import (ADAPTIVE, CapturePlan, configure_training, +from ..core.domain.capture_plan import (ADAPTIVE, STAGED, CapturePlan, configure_training, evidence_digest, select_task_training) from ..core.domain.reference import JointZeroReference from ..core.fitting.training_quality import (TRAINING_DECISION_VERSION, assess_training, @@ -15,11 +15,15 @@ from ..core.fitting.training_quality import (TRAINING_DECISION_VERSION, assess_t def resolve_capture_plan(profile, records, *, source_urdf=None, verify_decisions=False, require_complete=False): """Replay decisions in journal order, never from future or held-out rows.""" + if profile.quality.training_policy == STAGED: + from .staged_training import resolve_staged_plan + return resolve_staged_plan(profile, records, source_urdf=source_urdf, + verify_decisions=verify_decisions, require_complete=require_complete) base = configure_training(profile, profile.quality.training_policy) headers = [row for row in records if row.get("kind") == "session_start"] plan = CapturePlan.from_profile(base) if base.quality.training_policy != ADAPTIVE: - if any(row.get("kind") == "training_decision" for row in records): + if any(row.get("kind") in {"training_decision", "staged_training_decision"} for row in records): raise ValueError("adaptive_decisions_in_fixed_capture") if headers and "capture_plan" in headers[0] and headers[0]["capture_plan"] != plan.as_dict(): raise ValueError("capture_plan_header_changed") @@ -71,55 +75,93 @@ def resolve_capture_plan(profile, records, *, source_urdf=None, verify_decisions return current -def _assess_child(send, kwargs): +def _assess_child(connection): + cache = {} try: - send.send((assess_training(**kwargs), None)) - except Exception: - send.send((None, traceback.format_exc())) + while True: + kwargs = connection.recv() + if kwargs is None: + return + try: + if kwargs["profile"].quality.training_policy == STAGED: + from .staged_training import assess_staged_training + result = assess_staged_training(**kwargs, cache=cache) + else: + result = assess_training(**kwargs) + connection.send((result, None)) + except Exception: + connection.send((None, traceback.format_exc())) + except (EOFError, BrokenPipeError): + return finally: - send.close() + connection.close() class TrainingWorker: - """One bounded job at a time; spawning and fitting stay off the control loop.""" + """One reusable isolated worker; serialization and fitting stay off the loop.""" def __init__(self, timeout_seconds=120.): self.timeout_seconds = timeout_seconds self._closed = threading.Event() self._executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="training-check") self._future = None + self._process = self._connection = None + + def _stop(self): + process, connection = self._process, self._connection + self._process = self._connection = None + if connection is not None: + connection.close() + if process is not None and process.pid is not None: + if process.is_alive(): + process.terminate() + process.join(timeout=1.) + if process.is_alive(): + process.kill() + process.join(timeout=1.) + process.close() def _run(self, kwargs): - context = multiprocessing.get_context("spawn") - receive, send = context.Pipe(duplex=False) - process = context.Process(target=_assess_child, args=(send, kwargs), daemon=True) started = time.monotonic() try: - process.start() - send.close() + if self._process is None: + context = multiprocessing.get_context("spawn") + self._connection, child = context.Pipe(duplex=True) + self._process = context.Process(target=_assess_child, args=(child,), daemon=True) + try: + self._process.start() + finally: + child.close() + # A large serialization/write or reply must also obey cancellation + # and the deadline. Killing the child closes the peer and releases IO. + reply = Future() + connection = self._connection + def exchange(): + try: + connection.send(kwargs) + reply.set_result(connection.recv()) + except BaseException as error: + reply.set_exception(error) + threading.Thread(target=exchange, name="training-transport", daemon=True).start() while not self._closed.is_set(): - remaining = self.timeout_seconds - (time.monotonic() - started) + remaining = self.timeout_seconds - (time.monotonic()-started) if remaining <= 0: raise TimeoutError("training_assessment_timeout") - if receive.poll(min(.05, remaining)): - result, error = receive.recv() + if reply.done(): + result, error = reply.result() if error is not None: raise RuntimeError(error) return result - if not process.is_alive(): - raise RuntimeError(f"training_assessment_process_exited:{process.exitcode}") + if not self._process.is_alive(): + raise RuntimeError(f"training_assessment_process_exited:{self._process.exitcode}") + self._closed.wait(min(.05, remaining)) raise RuntimeError("training_assessment_cancelled") + except BaseException: + self._stop() + raise finally: - receive.close() - send.close() - if process.pid is not None: - if process.is_alive(): - process.terminate() - process.join(timeout=1.) - if process.is_alive(): - process.kill() - process.join(timeout=1.) - process.close() + if self._closed.is_set(): + self._stop() def poll(self, **kwargs): if self._closed.is_set(): @@ -132,5 +174,8 @@ class TrainingWorker: return future.result() def close(self): + if self._closed.is_set(): + return self._closed.set() - self._executor.shutdown(wait=False, cancel_futures=True) + self._executor.submit(self._stop) + self._executor.shutdown(wait=False) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py index 08e7ec0..b817fef 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py @@ -8,6 +8,8 @@ class FailureCategory(str, Enum): UNOBSERVABLE = "unobservable" REFERENCE_CHANGED = "reference_changed" DEVICE_FAULT = "device_fault" + NUMERICAL_SOLVER = "numerical_solver" + EVIDENCE_GAP = "evidence_gap" UNKNOWN = "unknown" @@ -17,22 +19,31 @@ RETRYABLE_PREPARATION_FAILURES = frozenset({ "insufficient_image_frames", "motion_candidates_missing_or_invalid", "image_motion_insufficient_independent_frames", - "image_motion_no_observable_image_model", "image_motion_validation_frame_unresolved", "image_motion_incomplete_candidate_family", - "motion_geometry_unresolved", }) def failure_category(reason): code = str(reason).split(":", 1)[0] + if code in {"preparation_sync_coverage_missing", "preparation_sync_endpoint_missing", + "preparation_sync_gap_too_long", "preparation_state_vector_invalid"}: + return FailureCategory.EVIDENCE_GAP + if code in {"image_motion_solver_budget_exhausted", "image_motion_optimizer_unresolved", + "image_motion_solver_timeout", "image_motion_frame_optimizer_unresolved"}: + return FailureCategory.NUMERICAL_SOLVER if code in {"image_motion_no_observable_image_model", "motion_geometry_unresolved", - "image_motion_families_not_distinguishable", "image_motion_shared_pose_unresolved"}: + "image_motion_families_not_distinguishable", "image_motion_shared_pose_unresolved", + "preparation_witness_unobservable", "preparation_witness_pose_uncertain", + "preparation_witness_endpoint_unresolved"}: return FailureCategory.UNOBSERVABLE if code in RETRYABLE_PREPARATION_FAILURES or code in {"sweep_acquisition", "joint_zero_samples_missing"}: return FailureCategory.MISSING_SAMPLES if code in {"motion_held_feedback_changed", "fixed_reference_moved", "joint_resume_reference_changed", - "parent_reference_pose_changed", "camera_extrinsics_changed"}: + "parent_reference_pose_changed", "camera_extrinsics_changed", + "preparation_witness_held_posture_changed", "preparation_witness_current_posture_changed", + "preparation_witness_reference_feedback_changed", "preparation_witness_camera_reference_changed", + "preparation_witness_source_pose_changed"}: return FailureCategory.REFERENCE_CHANGED if code in {"mechanical_stall", "sdk_disconnected", "hardware_fault", "feedback_stale", "duplicate_controller"}: return FailureCategory.DEVICE_FAULT @@ -42,18 +53,23 @@ def failure_category(reason): def permits_recovery(reasons, completed_retries): reasons = tuple(reasons) return completed_retries == 0 and bool(reasons) and all( - failure_category(reason) == FailureCategory.MISSING_SAMPLES - or str(reason).split(":", 1)[0] in RETRYABLE_PREPARATION_FAILURES for reason in reasons) + failure_category(reason) == FailureCategory.MISSING_SAMPLES for reason in reasons) class ZeroRecovery: def __init__(self): self._used = set() + def restore(self, task_key, joints): + self._used.add((task_key, tuple(joints))) + def take(self, motion, *, geometry_error, reports, referenced_joints): if motion.reference_reuse: return None - if not motion.zero_joints or set(motion.zero_joints) & set(referenced_joints): + if not motion.zero_joints or (set(motion.zero_joints) & set(referenced_joints) + and not getattr(motion, 'reference_check', False)): + return None + if getattr(motion, 'reference_check', False) and geometry_error: return None if geometry_error: # A partially installed multi-view model must not be refitted. diff --git a/src/linkerhand_calibration/test/fixtures/o30_pinky_handoff.json.gz b/src/linkerhand_calibration/test/fixtures/o30_pinky_handoff.json.gz new file mode 100644 index 0000000..269a6f4 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_pinky_handoff.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_pinky_preparation_density.json.gz b/src/linkerhand_calibration/test/fixtures/o30_pinky_preparation_density.json.gz new file mode 100644 index 0000000..23348ba Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_pinky_preparation_density.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_pinky_witness_sync.json.gz b/src/linkerhand_calibration/test/fixtures/o30_pinky_witness_sync.json.gz new file mode 100644 index 0000000..3a98eb1 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_pinky_witness_sync.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_revisit_feedback.json.gz b/src/linkerhand_calibration/test/fixtures/o30_revisit_feedback.json.gz new file mode 100644 index 0000000..27d9f8c Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_revisit_feedback.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_ring_mcp_ambiguous.json.gz b/src/linkerhand_calibration/test/fixtures/o30_ring_mcp_ambiguous.json.gz new file mode 100644 index 0000000..50765b9 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_ring_mcp_ambiguous.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_solver_budget.json.gz b/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_solver_budget.json.gz new file mode 100644 index 0000000..c572df8 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_solver_budget.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_uncertain_alternative.json.gz b/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_uncertain_alternative.json.gz new file mode 100644 index 0000000..41089fb Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_thumb_ip_uncertain_alternative.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_thumb_preparation_transition.json.gz b/src/linkerhand_calibration/test/fixtures/o30_thumb_preparation_transition.json.gz new file mode 100644 index 0000000..d5131da Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_thumb_preparation_transition.json.gz differ diff --git a/src/linkerhand_calibration/test/staged_capture_fixture.py b/src/linkerhand_calibration/test/staged_capture_fixture.py new file mode 100644 index 0000000..979cef8 --- /dev/null +++ b/src/linkerhand_calibration/test/staged_capture_fixture.py @@ -0,0 +1,72 @@ +"""Reuse the formal pixel/provenance fixture in complete staged visits.""" + +from collections import defaultdict +from copy import deepcopy + +from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training +from linkerhand_calibration.core.domain.motion_path import record_scan_identity +from linkerhand_calibration.core.domain.reference import JointZeroReference, build_joint_zero_reference +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.revisit import revisit_record +from linkerhand_calibration.runtime.staged_training import assess_staged_training, staged_snapshot +from o30_formal_fixture import formal_capture + + +def staged_capture(capture, directory): + fixed, source, original, hashes, camera_file, truth = formal_capture(capture, directory) + profile = configure_training(fixed, STAGED) + engine = CalibrationEngine(profile) + prologue, preparations, units, references, zero_rows = [], defaultdict(list), defaultdict(list), {}, defaultdict(list) + for row in original: + kind = row['kind'] + if kind == 'joint_zero_reference': + references[row['joint']] = JointZeroReference.from_record(row) + if kind == 'joint_zero_sample': + zero_rows[row['joint']].append(row) + if kind in {'session_start', 'rectified_camera_model', 'fixed_base_reference_locked'}: + prologue.append(row) + elif row.get('cycle') is not None and kind not in {'joint_zero_reference', 'joint_zero_sample'}: + units[record_scan_identity(row)].append(row) + else: + preparations[row.get('task_name', row.get('task_key'))].append(row) + prologue[0] = {**prologue[0], 'capture_plan': CapturePlan.from_profile(profile).as_dict(), + 'capture_schedule_version': engine.capture_schedule_version} + journal, frozen = prologue, None + stamp = max(r.get('image_stamp_ns', 0) for r in original)+1_000_000_000 + def retime(row, new_stamp): + row = deepcopy(row) + row['image_stamp_ns'] = new_stamp + if 'sample_id' in row: + row['sample_id'] = f"{row['view']}:{new_stamp}" + for item in row.get('pnp_observation_evidence', {}).values(): + item['candidate_diagnostics']['observation_stamp_ns'] = new_stamp + return row + for cycle in (0, 1, 3): + for task in profile.motion.tasks: + if cycle == 0: + journal.extend(preparations[task.key]) + else: + for name in task.joints: + rows = [] + for row in zero_rows[name]: + stamp += 33_333_333 + rows.append(retime(row, stamp)) + current = build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=1000+cycle) + journal.extend(rows) + journal.append(revisit_record(profile, references[name], current, cycle)) + for unit in engine.scan_units(): + if (unit.task_key, unit.cycle) == (task.key, cycle): + rows = units[unit.identity] + if cycle > 0: + stamps = sorted({r['image_stamp_ns'] for r in rows if 'image_stamp_ns' in r}) + offset = stamp+33_333_333-stamps[0] + rows = [retime(r, r['image_stamp_ns']+offset) if 'image_stamp_ns' in r else r for r in rows] + stamp = stamps[-1]+offset + journal.extend(rows) + if cycle == 1: + result = assess_staged_training(profile=profile, rows=staged_snapshot(profile, journal), + references=references, source_urdf=source) + assert result['decision']['decision'] == 'freeze', result['decision'] + frozen = result['frozen_training'] + journal.append(result['decision']) + return profile, source, journal, hashes, camera_file, truth, frozen diff --git a/src/linkerhand_calibration/test/test_capture_plan.py b/src/linkerhand_calibration/test/test_capture_plan.py index 4990f9f..c8be5eb 100644 --- a/src/linkerhand_calibration/test/test_capture_plan.py +++ b/src/linkerhand_calibration/test/test_capture_plan.py @@ -131,12 +131,15 @@ def test_fixed_and_adaptive_checkpoints_are_isolated(profile): def test_stage_timing_accounts_for_elapsed_time_once(): from linkerhand_calibration.runtime.timing_diagnostics import StageTiming timing = StageTiming(0.) + timing.mark_start(1.) timing.enter("geometry_solving", 2.) timing.enter("geometry_solving", 5.) timing.enter("steady_sampling", 6.) report = timing.as_dict(7.) assert report["stage_seconds"] == {"startup": 2., "geometry_solving": 4., "steady_sampling": 1.} assert sum(report["stage_seconds"].values()) == report["elapsed_seconds"] + timing.enter('terminal', 9.) + assert timing.as_dict(20.)['start_to_terminal_seconds'] == 8. def test_adaptive_resume_restarts_suffix_after_incomplete_dependency(profile): diff --git a/src/linkerhand_calibration/test/test_checkpoint_startup.py b/src/linkerhand_calibration/test/test_checkpoint_startup.py new file mode 100644 index 0000000..3c3af53 --- /dev/null +++ b/src/linkerhand_calibration/test/test_checkpoint_startup.py @@ -0,0 +1,66 @@ +"""Checkpoint verification has observable liveness and a bounded deadline.""" +from threading import Event, current_thread + +import pytest + +from linkerhand_calibration.runtime.startup import checkpoint_heartbeat +from test_unified_runner import LifecycleHarness + + +def test_loading_does_not_consume_device_or_service_deadlines(): + loading = {'state': 'PREPARING_CHECKPOINT'} + harness = LifecycleHarness([loading]*6 + [{'state': 'WAIT_DEVICE'}, {'state': 'READY'}, + {'state': 'COMPLETE'}], step=65.) + assert harness.run()['state'] == 'COMPLETE' + assert harness.starts == 1 + + +def test_alive_but_stuck_checkpoint_has_a_finite_loading_deadline(): + harness = LifecycleHarness([{'state': 'PREPARING_CHECKPOINT'}]*10, step=65.) + assert harness.run(checkpoint_timeout=200.)['reason'] == 'calibration_checkpoint_preparation_timeout' + assert harness.starts == 0 + + +def test_loading_does_not_hide_device_failure_after_loading(): + harness = LifecycleHarness([{'state': 'PREPARING_CHECKPOINT'}]*3 + + [{'state': 'WAIT_DEVICE'}]*4, step=65.) + assert harness.run()['reason'].startswith('calibration_device_not_ready:') + assert harness.starts == 0 + + +@pytest.mark.parametrize('interrupted', [False, True]) +def test_startup_heartbeat_stops_before_normal_callbacks_even_on_error(interrupted): + events, workers = [], [] + received = Event() + def publish(snapshot): + events.append(snapshot) + if current_thread().name == 'checkpoint-status': + workers.append(current_thread()) + received.set() + try: + with checkpoint_heartbeat(publish, enabled=True, interval=.01): + assert received.wait(1.) + assert all(s.state == 'PREPARING_CHECKPOINT' and s.motion.command is None for s in events) + if interrupted: + raise KeyboardInterrupt + except KeyboardInterrupt: + assert interrupted + assert workers and all(not w.is_alive() for w in workers) + + +def test_fresh_session_does_not_publish_checkpoint_status(): + with checkpoint_heartbeat(lambda _: pytest.fail('not resuming'), enabled=False): + pass + + +def test_background_publication_failure_is_reported_after_worker_cleanup(): + received, workers = Event(), [] + def publish(_): + if current_thread().name == 'checkpoint-status': + workers.append(current_thread()) + received.set() + raise ValueError('publisher unavailable') + with pytest.raises(RuntimeError, match='checkpoint_status_publication_failed'): + with checkpoint_heartbeat(publish, enabled=True, interval=.01): + assert received.wait(1.) + assert all(not worker.is_alive() for worker in workers) diff --git a/src/linkerhand_calibration/test/test_image_bundle_optimizer.py b/src/linkerhand_calibration/test/test_image_bundle_optimizer.py new file mode 100644 index 0000000..2e5f2d5 --- /dev/null +++ b/src/linkerhand_calibration/test/test_image_bundle_optimizer.py @@ -0,0 +1,179 @@ +"""Finite numerical recovery, real acquisition replay, and unchanged refusal gates.""" +from copy import deepcopy +from dataclasses import replace +import gzip +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.sparse import eye + +from linkerhand_calibration.core.geometry.tag_pose import image_bundle_optimizer as optimizer +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters + + +def result(status, cost=2., nfev=150, x=(.5,)): + return SimpleNamespace(status=status, success=status > 0, cost=cost, nfev=nfev, + x=np.array(x), fun=np.array([1.]), optimality=.1) + + +@pytest.mark.parametrize('terminal', ['exhausted', 'exception', 'worse', 'nonfinite']) +def test_unresolved_recovery_never_authorizes_or_repeats(monkeypatch, terminal): + first = result(0) + calls = [] + def solve(fun, initial, **kwargs): + calls.append((initial.copy(), kwargs)) + if len(calls) == 1: + return first + if terminal == 'exception': + raise ValueError('numerical failure') + return result(0 if terminal == 'exhausted' else 2, + cost=3. if terminal == 'worse' else 1., nfev=50, + x=(np.nan,) if terminal == 'nonfinite' else (.4,)) + monkeypatch.setattr(optimizer, 'least_squares', solve) + fit = optimizer.solve_image_bundle(lambda x: x, np.array([.9]), + sparsity=eye(1), scale=np.ones(1), limits=ImageMotionParameters(), bounds=([0.], [1.])) + assert not fit.success and fit.status == 0 + assert len(calls) == len(fit.image_solver_attempts) == 2 + assert calls[1][0] == first.x + assert calls[1][1]['tr_solver'] == 'exact' + assert calls[1][1]['max_nfev'] == 50 + assert calls[0][1]['bounds'] == calls[1][1]['bounds'] + for tolerance in ('ftol', 'xtol', 'gtol'): + assert calls[0][1][tolerance] == calls[1][1][tolerance] + + +@pytest.mark.parametrize('case', ['converged', 'large_graph', 'disabled', 'nonfinite']) +def test_ineligible_retry_does_not_spend_more_budget(monkeypatch, case): + first = result(2 if case == 'converged' else 0, + x=tuple(np.zeros(optimizer.MAXIMUM_DENSE_PARAMETERS+1)) if case == 'large_graph' else (np.nan if case == 'nonfinite' else .5,)) + calls = [] + monkeypatch.setattr(optimizer, 'least_squares', lambda *a, **k: calls.append(k) or first) + fit = optimizer.solve_image_bundle(lambda x: x, np.zeros(len(first.x)), + sparsity=eye(len(first.x)), scale=np.ones(len(first.x)), + limits=ImageMotionParameters(maximum_solver_recovery_evaluations=0 if case == 'disabled' else 50)) + assert fit is first and len(calls) == 1 and len(fit.image_solver_attempts) == 1 + + +@pytest.fixture(scope='module') +def exhausted_ip(): + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization + from linkerhand_calibration.runtime.motion_execution import MotionCommand + from linkerhand_calibration.runtime.passed_resume import restore_parent_models + from o30_capture_fixture import SOURCE + data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_thumb_ip_solver_budget.json.gz').read_bytes())) + rows, failure = data['records'], data['original_failure'] + profile = load_bundled_hand_profile('o30_right_18') + owner = BranchInitialization(profile, source_urdf=SOURCE) + restore_parent_models(owner.parent_references, + [r for r in rows if r['kind'] == 'motion_branch_initialization'], failure['session_epoch']) + owner.parent_references.verified[(failure['session_epoch'], failure['task_name'])] = next( + r for r in rows if r['kind'] == 'parent_reference_verified') + motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['thumb_ip'].command, + 200., task_key=failure['task_name'], reference_joints=('thumb_ip',)) + owner.begin(motion, session_epoch=failure['session_epoch'], motion_version=failure['motion_version']) + for row in rows: + if row['kind'] == 'motion_branch_observation' and row['task_name'] == failure['task_name']: + owner.observe(row) + request = owner.request('front', replace(motion, phase='joint_zero', zero_joints=('thumb_ip',)), + session_epoch=failure['session_epoch'], motion_version=failure['motion_version']+1) + assert not request.invalid_reason + assert tuple(f.evidence.stamp_ns for f in request.image_frames) == tuple(failure['source_image_stamps']) + return data, request, solve_initialization(request) + + +@pytest.mark.replay +def test_real_exhausted_candidate_is_solved_then_rejected_by_original_pixel_gate(exhausted_ip): + from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion + from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts + from linkerhand_calibration.runtime.motion_provenance import _check_measured_transfer_report + from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict, image_model_payload + data, request, resolved = exhausted_ip + assert data['original_failure']['hypotheses'][1]['training_solver_status'] == 0 + assert resolved.resolved, resolved.reason + winner, alternative = resolved.hypotheses + assert winner.training_accepted and len(winner.training_solver_attempts) == 1 + assert alternative.converged and not alternative.training_accepted + assert alternative.reason == 'image_motion_reprojection_failed' + assert [a.backend for a in alternative.training_solver_attempts] == ['sparse_lsmr', 'dense_exact'] + assert alternative.training_solver_attempts[0].status == 0 + assert alternative.training_solver_attempts[1].status > 0 + assert alternative.training_solver_evaluations <= 200 + assert winner.validation_quality[0].maximum_frame_rms_px < .34 + assert alternative.validation_quality[0].maximum_frame_rms_px > 1.5 + assert resolved.model.maximum_reprojection_error_px == 1.5 + assert set(resolved.model.training_stamps_ns).isdisjoint(resolved.model.validation_stamps_ns) + _, report = initialization_artifacts(request, resolved) + _check_measured_transfer_report(report) + assert image_motion_model_from_dict(report['frozen_image_model']) == resolved.model + for payload in report['evidence_payloads'].values(): + assert payload['image_solver_policy'] == optimizer.IMAGE_BUNDLE_SOLVER_POLICY + assert payload['image_solver_attempts'] == report['image_solver_attempts'] + # Audit replays the same raw pixels with the same deterministic two-stage solver. + replayed = resolve_image_motion(request.image_frames, request.relations, + geometry_constraints=request.geometry_constraints, source_hinges=request.source_hinges, + source_urdf_sha256=request.source_urdf_sha256) + assert replayed.resolved and image_model_sha256(image_model_payload(replayed.model)) == image_model_sha256(image_model_payload(resolved.model)) + + +@pytest.mark.replay +@pytest.mark.parametrize('eligible_winner', [True, False]) +def test_budget_exhaustion_still_vetoes_without_motion_retry(exhausted_ip, eligible_winner): + from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver + from linkerhand_calibration.runtime.zero_recovery import failure_category, FailureCategory, permits_recovery + from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh + _, request, resolved = exhausted_ip + winner, alternative = resolved.hypotheses + winner = replace(winner, training_accepted=eligible_winner) + alternative = replace(alternative, converged=False, reason='image_motion_optimizer_unresolved', + training_solver_status=0, training_accepted=False) + validation = [i for i, f in enumerate(request.image_frames) + if f.evidence.stamp_ns in resolved.model.validation_stamps_ns] + outcome = solver.select_image_hypotheses(request.image_frames, validation, + [winner, alternative], [resolved.model, resolved.model], + [np.zeros(len(validation))]*2, ImageMotionParameters()) + assert not outcome.resolved and outcome.model is None + assert outcome.reason == 'image_motion_solver_budget_exhausted' + assert failure_category(outcome.reason) == FailureCategory.NUMERICAL_SOLVER + assert not permits_recovery([outcome.reason], 0) + assert reason_zh({'reason': 'joint_zero_motion_unresolved:'+outcome.reason}, model_name='O30')[0] == 'OBS-SOLVER-118' + + +@pytest.mark.replay +def test_recovered_real_preparation_passes_formal_source_and_parent_contracts(exhausted_ip): + from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts + from linkerhand_calibration.runtime.motion_provenance import _initializations, validate_source_geometry + from linkerhand_calibration.profiles import load_bundled_hand_profile + from o30_capture_fixture import SOURCE + data, request, resolved = exhausted_ip + _, report = initialization_artifacts(request, resolved) + records = json.loads(json.dumps([*data['records'], report])) + profile = load_bundled_hand_profile('o30_right_18') + assert _initializations(records) + validate_source_geometry(profile, SOURCE, records) + + +@pytest.mark.replay +def test_solver_audit_tampering_rejected_before_source_replay(exhausted_ip): + from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts + from linkerhand_calibration.runtime.motion_provenance import _initializations + _, request, resolved = exhausted_ip + _, report = initialization_artifacts(request, resolved) + for field in ('image_solver_attempts', 'image_solver_policy'): + changed = deepcopy(report) + changed[field] = [] if field == 'image_solver_attempts' else 'ignore_unfinished' + with pytest.raises(ValueError, match='solver_report_changed'): + _initializations([changed]) + changed = json.loads(json.dumps(report)) + changed['hypotheses'][1]['training_solver_attempts'][1]['status'] = 0 + with pytest.raises(ValueError, match='solver_report_changed'): + _initializations([changed]) + changed = deepcopy(report) + changed.pop('image_solver_policy') + changed.pop('image_solver_attempts') + with pytest.raises(ValueError, match='solver_report_changed'): + _initializations([changed]) diff --git a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py index fa02a0d..59a7752 100644 --- a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py +++ b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py @@ -104,6 +104,28 @@ def test_unconfirmed_diagnostic_does_not_emit_formal_image_candidate_frame(monke assert [row["kind"] for row in rows] == ["diagnostic_observation_frame"] +@pytest.mark.parametrize('reason', [ + 'pose_image_motion_model_changed', + 'pose_image_motion_decision_invalid', + 'pose_observation_superseded', +]) +def test_failed_commit_keeps_original_reason_without_authorizing_missing_snapshot(monkeypatch, reason): + profile, capture, frame, branch = capture_rig(monkeypatch) + task = profile.motion.tasks[0] + role = branch.image_model.geometry[0].relation.child_role + evidence = SimpleNamespace(observation_snapshot=lambda *args: None) + monkeypatch.setattr(capture_module, 'select_motion_poses', + lambda *args, **kwargs: ({}, {role: reason}, evidence)) + motion = MotionCommand('joint_zero', profile.command.baseline_values, 40., + task_key=task.key, zero_joints=task.joints) + rows, _ = capture.consume(frame, motion) + event = next(row for row in rows if row['kind'] == 'pnp_branch_event' and row['tag_role'] == role) + assert event['reason'] == reason + assert event['pnp_observation_evidence']['selected_pose'] is None + assert not event['pnp_observation_evidence']['candidate_diagnostics'] + assert not any(row['kind'] in {'joint_zero_sample', 'pnp_candidate_frame'} for row in rows) + + @pytest.mark.parametrize("feedback_available", [True, False]) def test_diagnostic_hold_keeps_transient_raw_images_before_formal_steady_window(monkeypatch, feedback_available): profile, capture, frame, _ = capture_rig(monkeypatch) diff --git a/src/linkerhand_calibration/test/test_measured_transfer.py b/src/linkerhand_calibration/test/test_measured_transfer.py index 3bfcab3..db1c3d6 100644 --- a/src/linkerhand_calibration/test/test_measured_transfer.py +++ b/src/linkerhand_calibration/test/test_measured_transfer.py @@ -196,3 +196,83 @@ def test_ambiguity_has_an_actionable_operator_message(): model_name="O30") assert code == "OBS-POSE-AMBIGUOUS-117" assert "多个" in message and "独立观测" in action and "已通过的数据保留" in action + + +@pytest.fixture(scope="module") +def thumb_ip_uncertain_alternative(): + path = Path(__file__).parent / "fixtures/o30_thumb_ip_uncertain_alternative.json.gz" + data = json.loads(gzip.decompress(path.read_bytes())) + rows, failure = data["records"], data["original_failure"] + profile = load_bundled_hand_profile("o30_right_18") + owner = BranchInitialization(profile, source_urdf=SOURCE) + restore_parent_models(owner.parent_references, + [r for r in rows if r["kind"] == "motion_branch_initialization"], failure["session_epoch"]) + verified = next(r for r in rows if r["kind"] == "parent_reference_verified") + owner.parent_references.verified[(failure["session_epoch"], failure["task_name"])] = verified + spec = profile.motion.joint_zero_references["thumb_ip"] + motion = MotionCommand("zero_approach", spec.command, 200., + task_key=failure["task_name"], reference_joints=("thumb_ip",)) + owner.begin(motion, session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]) + for row in rows: + if row["kind"] == "motion_branch_observation": + owner.observe(row) + request = owner.request("front", replace(motion, phase="joint_zero", zero_joints=("thumb_ip",)), + session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]+1) + assert not request.invalid_reason + assert tuple(f.evidence.stamp_ns for f in request.image_frames) == tuple(failure["source_image_stamps"]) + return request, solve_initialization(request) + + +def test_real_ip_compares_uncertain_alternative_before_authorizing(thumb_ip_uncertain_alternative): + request, result = thumb_ip_uncertain_alternative + assert result.resolved, result.reason + winner, alternative = result.hypotheses + assert winner.training_accepted and not winner.reason + assert alternative.converged and not alternative.training_accepted + assert alternative.reason == "image_motion_geometry_uncertain" + assert alternative.training_solver_evaluations > 0 + assert alternative.training_quality and alternative.validation_quality + assert winner.comparison_adjusted_p_values[0][0] == 1 + assert winner.comparison_adjusted_p_values[0][1] <= .01 + assert winner.validation_quality[0].maximum_frame_rms_px < .464 + assert result.model.maximum_reprojection_error_px == 1.5 + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + frozen, report = initialization_artifacts(request, result) + assert frozen is not None + _check_measured_transfer_report(report) + assert image_motion_model_from_dict(report["frozen_image_model"]) == result.model + + +def test_uncertain_alternative_cannot_be_frozen(thumb_ip_uncertain_alternative): + from linkerhand_calibration.core.geometry.tag_pose.measured_transfer import transfer_geometry_checks + + request, result = thumb_ip_uncertain_alternative + alternative = result.hypotheses[1] + source = request.source_hinges[0] + constraint = request.geometry_constraints[0] + # Remove the inherited part to reconstruct the alternative's actual local + # bounds. Reusing valid geometry isolates the precision authorization gate. + bounds = (("thumb_ip", alternative.axis_uncertainty_95_rad[0][1]-source.child_axis_uncertainty_rad, + alternative.pivot_uncertainty_95_m[0][1]-source.child_point_uncertainty_m + -constraint.distance_m*source.child_axis_uncertainty_rad),) + with pytest.raises(ValueError, match="image_motion_geometry_uncertain"): + transfer_geometry_checks(result.model.geometry, request.geometry_constraints, request.source_hinges, bounds) + with pytest.raises(ValueError, match="image_motion_geometry_uncertain"): + replace(result.model, current_geometry_uncertainty=bounds) + + +def test_uncertain_alternative_still_vetoes_when_images_do_not_distinguish( + thumb_ip_uncertain_alternative, monkeypatch): + from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver + + request, result = thumb_ip_uncertain_alternative + # Isolate the selection boundary: equally good images, distinct poses, and + # the actual precision-ineligible hypothesis must not authorize the winner. + monkeypatch.setattr(solver, "equivalent_frame_selections", lambda *args: False) + validation = tuple(i for i, f in enumerate(request.image_frames) + if f.evidence.stamp_ns in result.model.validation_stamps_ns) + rejected = solver.select_image_hypotheses(request.image_frames, validation, + list(result.hypotheses), [result.model, result.model], + [np.zeros(len(validation)), np.zeros(len(validation))], ImageMotionParameters()) + assert not rejected.resolved and rejected.model is None + assert rejected.reason == "image_motion_families_not_distinguishable" diff --git a/src/linkerhand_calibration/test/test_motion_reference_provenance.py b/src/linkerhand_calibration/test/test_motion_reference_provenance.py index 6c24575..cdbdb87 100644 --- a/src/linkerhand_calibration/test/test_motion_reference_provenance.py +++ b/src/linkerhand_calibration/test/test_motion_reference_provenance.py @@ -268,17 +268,10 @@ def test_partial_resume_of_an_already_resumed_session_keeps_unverified_joint_map assert set(subsequent.references) == {first.joint} -def test_finalizer_requires_provenance_for_task_outside_articulated_candidate_selection(tmp_path, monkeypatch): - from pathlib import Path - from linkerhand_calibration.runtime.artifacts import finalization +def test_motion_gate_requires_provenance_outside_articulated_candidate_selection(): _, _, records = motion_capture() profile = load_bundled_hand_profile("o6_right_8") records = [row for row in records if row["kind"] != "motion_branch_initialization"] profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset())) - source = Path(__file__).resolve().parents[1]/"urdf/o6_right/linkerhand_o6_right.urdf" - monkeypatch.setattr(finalization, "fit_profile_calibration", lambda *a, **kw: pytest.fail("fit must not start")) with pytest.raises(ValueError, match="motion_branch_provenance_"): - finalization.finalize_profile_session(profile=profile, session_dir=tmp_path, - serial_number="TEST", source_urdf=source, protected_inputs={}, records=records, - require_motion_evidence=True) - assert not list(tmp_path.glob("*.urdf")) + validate_motion_provenance(profile, records, required=True, require_complete=True) diff --git a/src/linkerhand_calibration/test/test_o30_launch.py b/src/linkerhand_calibration/test/test_o30_launch.py index 3910722..230e8c3 100644 --- a/src/linkerhand_calibration/test/test_o30_launch.py +++ b/src/linkerhand_calibration/test/test_o30_launch.py @@ -3,6 +3,7 @@ from pathlib import Path import hashlib import yaml +import pytest from launch import LaunchContext from launch.actions import DeclareLaunchArgument @@ -62,6 +63,61 @@ def test_o30_real_launch_and_right_hand_settings(tmp_path,monkeypatch): assert not (tmp_path/'session/raw_samples.jsonl').exists() +@pytest.mark.parametrize('policy', ['fixed', 'staged_2_to_3', 'occlusion_aware_witness_v1']) +def test_session_profile_survives_real_launch_parameter_chain(tmp_path, monkeypatch, policy): + from dataclasses import replace + from linkerhand_calibration.product import ProductCalibrationContract + from linkerhand_calibration.core.domain.capture_plan import CapturePlan, configure_training + from linkerhand_calibration.profiles.preparation import configure_preparation + from linkerhand_calibration.runtime.profile_contract import resolve_runtime_profile + from linkerhand_calibration.runtime.ros.parameters import parameter_defaults + + monkeypatch.setenv('ROS_LOG_DIR', str(tmp_path/'logs')) + config = synthetic_product(tmp_path) + original = config.calibration_contract.typed_profile + profile = configure_training(original, 'fixed' if policy == 'fixed' else 'staged_2_to_3') + if policy == 'occlusion_aware_witness_v1': + profile = configure_preparation(profile, policy) + config = replace(config, calibration_contract=ProductCalibrationContract(declarative=profile)) + context = LaunchContext() + context.launch_configurations.update(dict(parse_launch_arguments(launch_command( + config, tmp_path/'session', record_bag=False, commands_enabled=False)[4:]))) + module = launch_module() + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + owner, = [action for action in module._launch_stack(context) + if isinstance(action, Node) and action.node_package == 'linkerhand_calibration' + and action.node_executable == 'three_camera_calibration_node'] + effective = parameter_defaults(original) + effective.update({p.name: p.value for p in to_parameters_list(context, 'o30_calibration', '', + evaluate_parameters(context, owner._Node__parameters))}) + restored = resolve_runtime_profile(original, effective.__getitem__) + assert CapturePlan.from_profile(restored).as_dict() == CapturePlan.from_profile(profile).as_dict() + assert restored.motion.preparation_witnesses == profile.motion.preparation_witnesses + assert not (tmp_path/'session/raw_samples.jsonl').exists() + if policy == 'occlusion_aware_witness_v1': + assert len(restored.motion.preparation_witnesses) == 4 + # Reproduce the real omitted-parameter failure, then explicit removal/tampering. + for name, replacement in [('preparation_witnesses_json', ''), + ('preparation_witnesses_json', '{}'), ('training_policy', 'fixed')]: + with pytest.raises(ValueError, match='runtime_capture_plan_mismatch'): + resolve_runtime_profile(original, {**effective, name: replacement}.__getitem__) + + +def test_runtime_profile_preserves_legacy_yaml_and_rejects_malformed_overrides(): + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.profile_contract import resolve_runtime_profile + from linkerhand_calibration.runtime.ros.parameters import parameter_defaults + from linkerhand_calibration.core.domain.capture_plan import CapturePlan + profile = load_bundled_hand_profile('o30_right_18') + defaults = parameter_defaults(profile) + assert CapturePlan.from_profile(resolve_runtime_profile(profile, defaults.__getitem__)) == CapturePlan.from_profile(profile) + for encoded in ['[1]', '{', '{"ring_mcp_pitch": {}}']: + with pytest.raises(ValueError, match='invalid preparation_witnesses_json'): + resolve_runtime_profile(profile, {**defaults, 'preparation_witnesses_json': encoded}.__getitem__) + + def test_parent_model_preparation_freezes_branch_without_replacing_joint_zero(tmp_path,monkeypatch): from types import SimpleNamespace from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator diff --git a/src/linkerhand_calibration/test/test_observer_pose.py b/src/linkerhand_calibration/test/test_observer_pose.py new file mode 100644 index 0000000..c28de5c --- /dev/null +++ b/src/linkerhand_calibration/test/test_observer_pose.py @@ -0,0 +1,91 @@ +"""Independent observer images may constrain a family, never waive ambiguity.""" + +from dataclasses import replace +import math + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe +from linkerhand_calibration.core.geometry.tag_pose.observer_pose import fit_observer_pose +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + + +@pytest.fixture(scope='module') +def observer_images(): + matrix = np.array([[1600., 0, 640.], [0, 1600., 480.], [0, 0, 1.]]) + parents, candidates, corners = [], [], [] + relative = Rotation.identity() + translation = np.array([.045, .02, 0.]) + for theta in np.linspace(-35, 35, 48): + rotation = Rotation.from_euler('xy', [theta, theta*.4], degrees=True) + origin = np.array([.015, -.02, .5]) + parents.append(SquareTagPose(tuple(rotation.as_quat()), tuple(origin), 0.)) + points = rotation.apply(relative.apply(square_object_points(.016))+translation)+origin + projected = points@matrix.T + pixels = projected[:, :2]/projected[:, 2:] + corners.append(pixels) + candidates.append(tuple(solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=matrix))) + return parents, candidates, corners, [matrix]*48, .016, tuple(range(1, 49)) + + +def test_all_legal_candidates_use_fixed_split_and_propagate_parent_uncertainty(observer_images): + inherited = (math.radians(.05), .00005) + result = fit_observer_pose(*observer_images, inherited=inherited) + assert np.allclose(result.translation_xyz_m, [.045, .02, 0.], atol=1e-8) + assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-6 + assert result.rotation_uncertainty_rad >= inherited[0] + assert result.translation_uncertainty_m >= inherited[1] + assert set(result.training_stamps).isdisjoint(result.validation_stamps) + assert result.training_stamps == tuple(range(1, 49, 2)) + + +@pytest.mark.parametrize('fault', ['missing', 'moving', 'installation', 'unobservable', 'uncertain_parent']) +def test_invalid_observer_evidence_cannot_authorize_a_pose(observer_images, fault): + parents, candidates, corners, matrices, size, stamps = observer_images + inherited = (0., 0.) + if fault == 'missing': + parents, candidates, corners, matrices, stamps = [v[:10] for v in (parents, candidates, corners, matrices, stamps)] + elif fault in {'moving', 'installation'}: + corners = [np.array(pixels, copy=True) for pixels in corners] + for i in (range(1, len(corners), 2) if fault == 'moving' else range(len(corners)//2, len(corners))): + corners[i] += [10., -10.] + elif fault == 'unobservable': + parents = [parents[0]]*len(parents) + else: + inherited = (math.radians(2), .01) + with pytest.raises(ValueError, match='observer_'): + fit_observer_pose(parents, candidates, corners, matrices, size, stamps, inherited=inherited) + + +@pytest.mark.integration +def test_measured_observer_pose_resolves_only_a_separated_hinge_family(observer_images): + from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion + from test_motion_image_diagnostics import frames, RELATIONS + motion = frames(alternatives=True) + unresolved = resolve_image_motion(motion, RELATIONS) + assert not unresolved.resolved and unresolved.reason == 'image_motion_families_not_distinguishable' + measured = fit_observer_pose(*observer_images) + stationary = tuple(replace(motion[0], evidence=replace(motion[0].evidence, + stamp_ns=10_000_000_000+i*30_000_000)) for i in range(10)) + bridge = SharedTagPoseBridge(RELATIONS[0], measured.quaternion_xyzw, measured.translation_xyz_m, + stationary, rotation_uncertainty_rad=measured.rotation_uncertainty_rad, + translation_uncertainty_m=measured.translation_uncertainty_m) + result = resolve_image_motion(motion, RELATIONS, pose_bridges=(bridge,)) + assert result.resolved, result + assert all(c.status == 'consistent' for h in result.hypotheses if h.branches == result.model.branches + for c in h.pose_bridge_checks) + + +def test_observer_roles_do_not_change_the_current_task_or_require_visibility(): + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.motion_execution import MotionCommand + from linkerhand_calibration.runtime.observer_reference import observer_roles + profile = load_bundled_hand_profile('o30_right_18') + spec = profile.motion.joint_zero_references['middle_pip'] + motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('middle_pip',)) + assert 'middle_dip' in observer_roles(profile, motion) + assert observer_roles(profile, replace(motion, phase='sweep')) == set() + assert motion.reference_joints == ('middle_pip',) diff --git a/src/linkerhand_calibration/test/test_observer_reference.py b/src/linkerhand_calibration/test/test_observer_reference.py new file mode 100644 index 0000000..ab0e361 --- /dev/null +++ b/src/linkerhand_calibration/test/test_observer_reference.py @@ -0,0 +1,212 @@ +"""Raw observer pixels traverse the runtime bridge admission boundary.""" + +from copy import deepcopy +from dataclasses import asdict, replace +from types import SimpleNamespace as NS + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion +from linkerhand_calibration.runtime.observer_reference import ObserverBridgeInput, build_observer_bridge +from linkerhand_calibration.runtime.branch_initialization import BranchInitializationRequest, initialization_artifacts, solve_initialization +from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 +from test_motion_image_diagnostics import frames, RELATIONS, MATRIX + + +@pytest.fixture(scope='module') +def observer_source(): + source = frames() + # A tilted upstream axis provides genuinely different camera directions; + # rolling a frontal plane in place must remain ambiguous. + turn, origin = Rotation.from_euler('y', 25, degrees=True), np.array([0., 0., .8]) + transformed = [] + for frame in source: + roles, observations = [], [] + for role, observation in zip(frame.evidence.roles, frame.observations): + pose = role.candidates[0] + rotation = turn*Rotation.from_quat(pose.quaternion_xyzw) + if role.role == 'moving': + rotation = rotation*Rotation.from_euler('x', 20, degrees=True) + translation = origin+turn.apply(np.asarray(pose.translation_xyz_m)-origin) + pose = replace(pose, quaternion_xyzw=tuple(rotation.as_quat()), translation_xyz_m=tuple(translation)) + projected = (rotation.apply(square_object_points(.016))+translation)@np.asarray(MATRIX).T + roles.append(replace(role, candidates=(pose,))) + observations.append(replace(observation, corners_xy=tuple(map(tuple, projected[:, :2]/projected[:, 2:])))) + transformed.append(replace(frame, evidence=replace(frame.evidence, roles=tuple(roles)), observations=tuple(observations))) + source = tuple(transformed) + profile = NS(command=NS(unit='u8', command_index_by_joint={'hinge': 0, 'distal': 1}), + acquisition=NS(fixed_reference_minimum_frames=10), + motion=NS(joint_zero_references={'distal': NS(command=(255., 0.))}), + measurement=NS(measurements={'distal': NS(view='view', parent_role='moving', child_role='terminal')}), + vision=NS(views=(NS(name='view', tags=(NS(role='terminal', size_m=.016),)),))) + rows = [] + for i, frame in enumerate(source): + stamp = frame.evidence.stamp_ns + tags = {} + for role, observation in zip(frame.evidence.roles, frame.observations): + tags[role.role] = dict(tag_role=role.role, tag_size_m=.016, corners_xy=observation.corners_xy, + selected_pose=asdict(role.candidates[0]), reprojection_valid_candidates=[asdict(p) for p in role.candidates], + candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=role.frozen)) + parent = frame.evidence.roles[1].candidates[0] + rotation = Rotation.from_quat(parent.quaternion_xyzw) + points = rotation.apply(Rotation.from_euler('x', 20, degrees=True).apply(square_object_points(.016)) + + [.03, .01, -.005]) + parent.translation_xyz_m + projected = points@np.asarray(MATRIX).T + pixels = projected[:, :2]/projected[:, 2:] + candidates = solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=MATRIX) + tags['terminal'] = dict(tag_role='terminal', tag_size_m=.016, corners_xy=pixels.tolist(), + selected_pose=None, reprojection_valid_candidates=[asdict(p) for p in candidates], + candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=False)) + rows.append(dict(kind='motion_branch_observation', image_stamp_ns=stamp, view='view', task_name='upstream', + zero_joints=['hinge'], session_epoch=1, motion_version=1, camera_matrix=MATRIX, tags=tags, + input_is_rectified=True, sample_phase='zero_approach', + command_vector=[i*255/(len(source)-1), 0.], feedback_vector=[i*255/(len(source)-1), 0.])) + request = BranchInitializationRequest('upstream', 'view', ('hinge',), 1, 1, 1, + tuple(f.evidence for f in source), RELATIONS, (), ('moving',), + tuple((str(r['image_stamp_ns']), source_frame_sha256(r)) for r in rows), + image_frames=source, image_geometry=True) + result = resolve_image_motion(source, RELATIONS) + assert result.resolved + _, report = initialization_artifacts(request, result) + endpoints = [] + for i in range(10): + row = deepcopy(rows[-1]) + row.update(task_name='downstream', zero_joints=['distal'], motion_version=2, + image_stamp_ns=row['image_stamp_ns']+(i+1)*30_000_000) + for item in row['tags'].values(): + item['candidate_diagnostics']['observation_stamp_ns'] = row['image_stamp_ns'] + row['tags']['moving']['candidate_diagnostics']['branch_frozen'] = True + endpoints.append(row) + return profile, ObserverBridgeInput('distal', result.model, report, tuple(rows), tuple(endpoints), (1,)), request + + +def test_raw_bridge_keeps_original_corners_candidates_and_fixed_image_split(observer_source): + profile, source, _ = observer_source + original = source_frame_sha256(source.rows[0]) + bridge, record = build_observer_bridge(profile, source) + assert np.allclose(bridge.translation_xyz_m, [.03, .01, -.005], atol=1e-6) + assert (Rotation.from_quat(bridge.quaternion_xyzw).inv()*Rotation.from_euler('x', 20, degrees=True)).magnitude() < 1e-5 + assert record['source_frame_hashes'][str(source.rows[0]['image_stamp_ns'])] == original + assert source_frame_sha256(source.rows[0]) == original + assert set(record['pose']['training_stamps']).isdisjoint(record['pose']['validation_stamps']) + + +@pytest.mark.parametrize('fault', ['feedback', 'command', 'size', 'missing']) +def test_bad_observer_evidence_is_not_a_new_reference(observer_source, fault): + profile, source, request = observer_source + rows = deepcopy(source.rows) + if fault == 'feedback': + rows[-1]['feedback_vector'][1] = 10 + elif fault == 'command': + rows[-1]['command_vector'][1] = 10 + elif fault == 'size': + rows[-1]['tags']['terminal']['tag_size_m'] *= 2 + else: + del rows[-1]['tags']['terminal'] + invalid = replace(source, rows=tuple(rows)) + with pytest.raises((ValueError, KeyError)): + build_observer_bridge(profile, invalid) + if fault == 'missing': + # Missing optional pixels do not change this upstream hinge's evidence. + result = solve_initialization(replace(request, observer_sources=(invalid,), observer_profile=profile)) + assert result.resolved and result.observer_rejections and not result.observer_pose_bridges + + +@pytest.fixture(scope='module') +def observer_audit(observer_source): + from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation + from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates + from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose + profile, source, _ = observer_source + parent = source.rows[-1]['tags']['moving']['selected_pose'] + parent_rotation = Rotation.from_quat(parent['quaternion_xyzw']) + parent_point = np.asarray(parent['translation_xyz_m']) + relation = MotionRelation('distal', 'moving', 'terminal') + rows, images = [], [] + for index, theta in enumerate(np.linspace(np.deg2rad(35), 0., 49)): + stamp = 5_000_000_000+index*50_000_000+(500_000_000 if index == 48 else 0) + arc = Rotation.from_rotvec([0., theta, 0.]) + poses = (('moving', parent_rotation, parent_point), + ('terminal', parent_rotation*arc*Rotation.from_euler('x', 20, degrees=True), + parent_point+parent_rotation.apply(arc.apply([.03, .01, -.005])))) + roles, observations, tags = [], [], {} + for role, rotation, point in poses: + projected = (rotation.apply(square_object_points(.016))+point)@np.asarray(MATRIX).T + corners = tuple(map(tuple, projected[:, :2]/projected[:, 2:])) + pose = SquareTagPose(tuple(rotation.as_quat()), tuple(point), .1) + roles.append(RoleCandidates(role, (pose,), role == 'moving')) + observations.append(ImageRoleObservation(role, .016, corners)) + tags[role] = dict(tag_role=role, tag_size_m=.016, corners_xy=corners, selected_pose=asdict(pose), + reprojection_valid_candidates=[asdict(pose)], + candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=role == 'moving')) + images.append(ImageMotionFrame(MotionEvidenceFrame(stamp, tuple(roles)), MATRIX, tuple(observations))) + rows.append(dict(kind='motion_branch_observation', image_stamp_ns=stamp, view='view', task_name='downstream', + zero_joints=['distal'], session_epoch=1, motion_version=2, camera_matrix=MATRIX, tags=tags, + input_is_rectified=True, sample_phase='zero_approach', command_vector=[255., float(theta)], + feedback_vector=[255., float(theta)])) + endpoints = [] + for index in range(10): + row = deepcopy(rows[-1]) + row['image_stamp_ns'] = rows[-2]['image_stamp_ns']+(index+1)*30_000_000 + for tag in row['tags'].values(): + tag['candidate_diagnostics']['observation_stamp_ns'] = row['image_stamp_ns'] + endpoints.append(row) + source = replace(source, endpoint_rows=tuple(endpoints)) + request = BranchInitializationRequest('downstream', 'view', ('distal',), 1, 2, 1, + tuple(f.evidence for f in images), (relation,), tuple(source.parent_report['evidence_ids'].items()), + ('moving', 'terminal'), tuple((str(r['image_stamp_ns']), source_frame_sha256(r)) for r in rows), + image_frames=tuple(images), image_geometry=True, observer_sources=(source,), observer_profile=profile) + result = solve_initialization(request) + assert result.resolved and result.observer_pose_bridges, result.reason + _, report = initialization_artifacts(request, result) + evidence = {report['evidence_ids'][role]: payload for report in (source.parent_report, report) + for role, payload in report['evidence_payloads'].items()} + records = (*source.rows, source.parent_report, *rows[:-1], *endpoints, rows[-1], report) + return profile, records, evidence + + +def test_observer_pixels_and_frozen_model_survive_journal_readback(observer_audit): + from linkerhand_calibration.runtime.observer_reference import validate_observer_references + from linkerhand_calibration.runtime.motion_provenance import _initializations + profile, records, evidence = observer_audit + assert _initializations(records) == evidence + validate_observer_references(profile, records, evidence) + + +def test_resume_retains_bound_observer_images_without_inventing_missing_pixels(observer_source): + from linkerhand_calibration.runtime.observer_reference import restore_observer_records, observer_inputs + profile, source, _ = observer_source + profile = NS(**vars(profile), vision_motion=False) + owner = NS(profile=profile, zero_joints=('distal',), observer_records={}, + tag_feedback_channels={'moving': {0}, 'terminal': {0, 1}}, + parent_references=NS(models={(2, 'view', 'hinge'): (NS(image_model=source.parent_model), source.parent_report)})) + assert not observer_inputs(owner, 'view', source.endpoint_rows, set(), 2) + rows = deepcopy(source.rows) + del rows[0]['tags']['terminal'] + restore_observer_records(owner, (*rows, *source.endpoint_rows)) + restored, = observer_inputs(owner, 'view', source.endpoint_rows, set(), 2) + assert restored.rows == rows[1:] + assert len(owner.observer_records[(1, 'upstream', 'view')]) == len(rows) + assert source_frame_sha256(restored.rows[0]) == source_frame_sha256(rows[1]) + + +@pytest.mark.parametrize('fault,reason', [('pixels', 'source_pixels_changed'), + ('sampling', 'source_sampling_changed'), ('installation', 'parent_installation_changed')]) +def test_observer_audit_rejects_changed_sources_without_refitting_whole_hand(observer_audit, fault, reason): + from linkerhand_calibration.runtime.observer_reference import validate_observer_references + profile, records, evidence = observer_audit + records, evidence = deepcopy(records), deepcopy(evidence) + payload = next(p for p in evidence.values() if p.get('observer_pose_bridges')) + bridge = payload['observer_pose_bridges'][0] + if fault == 'pixels': + row = next(r for r in records if str(r.get('image_stamp_ns')) in bridge['source_frame_hashes']) + row['tags']['terminal']['corners_xy'][0][0] += .1 + elif fault == 'sampling': + bridge['source_frame_hashes'].pop(next(iter(bridge['source_frame_hashes']))) + else: + bridge['parent_model_sha256'] = '0'*64 + with pytest.raises(ValueError, match=reason): + validate_observer_references(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_preparation_density.py b/src/linkerhand_calibration/test/test_preparation_density.py new file mode 100644 index 0000000..2cc14c9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_preparation_density.py @@ -0,0 +1,141 @@ +"""Replay real PIP/DIP pixels with a budget fixed before either fit. + +This is an offline counterfactual, not a passed capture: the new parent model +is projected onto the original DIP images. Original journals and frozen zeros +are never overwritten, and no artifact publication is authorized here. +""" +from copy import deepcopy +from dataclasses import asdict, replace +import gzip +import json +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.domain.capture_plan import configure_training +from linkerhand_calibration.core.domain.reference import JointZeroReference +from linkerhand_calibration.core.geometry.image_motion_replay import frozen_image_model +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( + ImageMotionParameters, resolve_image_motion, select_image_motion_frame, +) +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceParameters +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_image_budget +from linkerhand_calibration.runtime.branch_initialization import ( + BranchInitialization, solve_initialization, initialization_artifacts, +) +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.passed_resume import restore_parent_models +from linkerhand_calibration.runtime.shared_tag_reference import bridge_frames +from o30_capture_fixture import SOURCE + + +@pytest.fixture(scope='module') +def replay(): + data = json.loads(gzip.decompress((Path(__file__).parent / + 'fixtures/o30_pinky_preparation_density.json.gz').read_bytes())) + profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), + 'staged_2_to_3'), 'occlusion_aware_witness_v1') + owner = BranchInitialization(profile, source_urdf=SOURCE) + old = data['pip_report'] + reference = JointZeroReference.from_record(old['shared_tag_pose_bridges'][0]['source_reference']) + restore_parent_models(owner.parent_references, [data['source_report']], 1) + for row in data['source_window']: + owner.parent_references.handoff.observe(row) + def request(joint, report, rows, references=None): + spec = profile.motion.joint_zero_references[joint] + motion = MotionCommand('zero_approach', spec.command, 200., + task_key=report['task_name'], reference_joints=(joint,)) + owner.begin(motion, session_epoch=1, motion_version=report['motion_version']) + for row in rows: + owner.observe(row) + result = owner.request('side', replace(motion, phase='joint_zero', zero_joints=(joint,)), + session_epoch=1, motion_version=report['motion_version']+1, zero_references=references) + assert not result.invalid_reason + return result + pip_request = request('pinky_pip', old, data['pip_rows'], {reference.joint: reference}) + pip = solve_initialization(pip_request) + assert pip.resolved, pip.reason + _, pip_report = initialization_artifacts(pip_request, pip) + owner.accept(pip_request, pip) + # Geometry extraction requires current-pose admission. This offline marker + # only exercises the request builder, not the full pose/zero authorization. + owner.parent_references.verified[(1, 'pinky_dip_side')] = {'offline_counterfactual': True} + rows = deepcopy(data['dip_rows']) + for row in rows: + image = bridge_frames([row], pip.model.relations[0])[0] + selected = select_image_motion_frame(pip.model, image) + assert selected.resolved, selected.reason + tag = row['tags']['pinky_pip'] + tag['selected_pose'] = asdict(next(s.pose for s in selected.selections if s.role == 'pinky_pip')) + tag['frozen_image_model'] = pip_report['frozen_image_model'] + tag['image_model_sha256'] = pip_report['image_model_sha256'] + tag['candidate_diagnostics']['motion_evidence_id'] = pip_report['evidence_ids']['pinky_pip'] + dip_request = request('pinky_dip', data['dip_report'], rows) + dip = solve_initialization(dip_request) + return data, profile, pip_request, pip, dip_request, dip + + +@pytest.mark.replay +def test_real_parent_child_precision_with_disjoint_more_complete_evidence(replay): + data, _, pip_request, pip, dip_request, dip = replay + assert data['dip_report']['reason'] == 'image_motion_no_observable_image_model' + assert all(h['reason'] == 'image_motion_geometry_uncertain' for h in data['dip_report']['hypotheses']) + assert dip.resolved, dip.reason + for request, result in ((pip_request, pip), (dip_request, dip)): + assert 200 < len(request.frames) <= 257 + model = result.model + assert len(model.training_stamps_ns) > 100 + assert len(model.validation_stamps_ns) > 100 + assert not set(model.training_stamps_ns) & set(model.validation_stamps_ns) + assert set(model.training_stamps_ns) | set(model.validation_stamps_ns) == set(model.source_stamps_ns) + winner = result.hypotheses[result.selected_hypothesis] + assert all(v <= MotionEvidenceParameters().maximum_split_axis_difference_rad + for _, v in winner.axis_uncertainty_95_rad) + assert model.maximum_reprojection_error_px == 1.5 + _, report = initialization_artifacts(request, result) + assert frozen_image_model(report) == model + assert all(p < .01 for _, p in dip.hypotheses[dip.selected_hypothesis].comparison_adjusted_p_values) + + +@pytest.mark.replay +def test_dense_child_does_not_relabel_coarse_frozen_parent(replay): + data, _, _, _, request, _ = replay + from linkerhand_calibration.core.geometry.tag_pose.source_hinge import SourceHinge + old = data['pip_report'] + source = SourceHinge(frozen_image_model(old).geometry[0], old['evidence_ids']['pinky_pip'], + old['image_model_sha256'], *old['geometry_uncertainty'][0][1:]) + result = resolve_image_motion(request.image_frames, request.relations, + parameters=request.image_parameters, source_hinges=(source,), + geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256) + assert not result.resolved + assert result.reason == 'image_motion_families_not_distinguishable' + + +def test_preparation_budget_does_not_expand_multi_joint_graph_or_other_profiles(): + profile = configure_training(load_bundled_hand_profile('o30_right_18'), 'staged_2_to_3') + assert preparation_image_budget(profile) == (128, ImageMotionParameters()) + dense = configure_preparation(profile, 'occlusion_aware_witness_v1') + assert preparation_image_budget(dense)[0] == 256 + assert preparation_image_budget(dense, joint_count=4) == (128, ImageMotionParameters()) + with pytest.raises(ValueError, match='limits_invalid'): + ImageMotionParameters(maximum_training_frames=145) + + +@pytest.mark.replay +def test_sampling_budget_is_bound_and_cannot_be_removed_on_readback(replay): + from linkerhand_calibration.runtime.motion_provenance import _initializations, _check_image_initialization + from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import DENSE_PREPARATION_SAMPLING + data, _, request, result, _, _ = replay + _, report = initialization_artifacts(request, result) + assert report['image_sampling_policy'] == DENSE_PREPARATION_SAMPLING + assert _initializations([*data['pip_rows'], report]) + changed = deepcopy(report) + changed.pop('image_sampling_policy') + with pytest.raises(ValueError, match='binding_changed'): + _initializations([*data['pip_rows'], changed]) + payload = deepcopy(next(iter(report['evidence_payloads'].values()))) + payload.pop('image_sampling_policy') + observations = {(r['view'], r['image_stamp_ns']): r for r in data['pip_rows']} + with pytest.raises(ValueError, match='support_or_roles_changed'): + _check_image_initialization(payload, observations) diff --git a/src/linkerhand_calibration/test/test_preparation_sync.py b/src/linkerhand_calibration/test/test_preparation_sync.py new file mode 100644 index 0000000..74597eb --- /dev/null +++ b/src/linkerhand_calibration/test/test_preparation_sync.py @@ -0,0 +1,176 @@ +"""Real capture gaps must not become identity changes or bypass geometric gates.""" +from copy import deepcopy +from dataclasses import replace +import gzip +import json +from pathlib import Path + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.preparation import configure_preparation +from linkerhand_calibration.core.domain.capture_plan import configure_training +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.preparation_witness import select_witness_rows, build_preparation_witness +from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 +from linkerhand_calibration.runtime.preparation_witness_evidence import validate_preparation_witnesses +from o30_capture_fixture import SOURCE + + +@pytest.fixture(scope='module') +def real_capture(): + data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_pinky_witness_sync.json.gz').read_bytes())) + profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), + 'staged_2_to_3'), 'occlusion_aware_witness_v1') + owner = BranchInitialization(profile, source_urdf=SOURCE) + failure = data['failure'] + motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['pinky_mcp_pitch'].command, + 200., task_key=failure['task_name'], reference_joints=('pinky_mcp_pitch',)) + owner.begin(motion, session_epoch=failure['session_epoch'], motion_version=failure['motion_version']) + for row in data['rows']: + if row.get('preparation_witness_joint'): + owner.witness_capture.observe(row) + elif row['motion_version'] == failure['motion_version']: + owner.observe(row) + request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('pinky_mcp_pitch',)), + session_epoch=failure['session_epoch'], motion_version=failure['motion_version']+1) + assert not request.invalid_reason + return profile, data, request + + +@pytest.fixture(scope='module') +def replayed(real_capture): + profile, data, request = real_capture + result = solve_initialization(request) + assert result.resolved, result.reason + _, report = initialization_artifacts(request, result) + return result, json.loads(json.dumps([*data['rows'], report])) + + +@pytest.mark.replay +def test_real_gap_preserves_all_pixels_and_independent_acceptance(real_capture, replayed): + profile, _, request = real_capture + source = request.witness_sources[0] + assert len(source.rows) == 219 + missing = [r for r in source.rows if r['command_vector'] is None] + assert len(missing) == 2 + result, records = replayed + decision = result.preparation_witnesses[0] + assert decision['decision'] == 'independent_before_fit' + assert decision['reason'] == 'insufficient_rotation_evidence' + assert set(decision['source_frame_hashes']) == {str(r['image_stamp_ns']) for r in source.rows} + assert decision['sample_admission']['rejected_frames'] == [ + {'image_stamp_ns': r['image_stamp_ns'], 'missing': ['command_vector']} for r in missing] + assert not set(r['image_stamp_ns'] for r in missing) & set(decision['selected_image_stamps']) + assert all(r['command_vector'] is None for r in missing) # No imputation or mutation. + assert result.hypotheses[result.selected_hypothesis].comparison_adjusted_p_values[0][1] < .01 + evidence = _initializations(records) + validate_preparation_witnesses(profile, records, evidence) + + +@pytest.mark.parametrize('fault', ['held_feedback_on_missing_command', 'held_command_on_missing_feedback', + 'epoch', 'camera', 'vector_shape', 'nonfinite', 'endpoint', 'long_gap', 'many_missing']) +def test_missing_samples_cannot_hide_invalid_evidence(real_capture, fault): + profile, _, request = real_capture + source = request.witness_sources[0] + rows = deepcopy(source.rows) + index = next(i for i, r in enumerate(rows) if r['command_vector'] is None) + if fault == 'held_feedback_on_missing_command': + rows[index]['feedback_vector'][10] += 8 + elif fault == 'held_command_on_missing_feedback': + rows[index]['command_vector'] = list(profile.motion.preparation_witnesses[source.joint].command) + rows[index]['command_vector'][10] += 1 + rows[index]['feedback_vector'] = None + elif fault == 'epoch': + rows[index]['session_epoch'] += 1 + elif fault == 'camera': + rows[index]['camera_matrix'][0][0] += 1 + elif fault == 'vector_shape': + rows[index]['feedback_vector'] = [1.] + elif fault == 'nonfinite': + rows[index]['feedback_vector'][10] = float('nan') + elif fault == 'endpoint': + rows[-1]['command_vector'] = None + elif fault == 'long_gap': + for row in rows[index+2:]: + row['image_stamp_ns'] += 300_000_000 + else: + for row in rows[20:40]: + row['command_vector'] = None + with pytest.raises(ValueError): + select_witness_rows(profile, replace(source, rows=tuple(rows))) + + +def test_preflight_declines_before_any_auxiliary_fit(real_capture, monkeypatch): + from linkerhand_calibration.runtime import preparation_witness as module + profile, _, request = real_capture + monkeypatch.setattr(module, 'resolve_image_motion', lambda *a, **k: pytest.fail('auxiliary fitting must not start')) + bridge, record = build_preparation_witness(profile, request.witness_sources[0]) + assert bridge is None and record['decision'] == 'independent_before_fit' + + +def test_incomplete_candidate_is_not_proof_of_insufficient_motion(monkeypatch): + from linkerhand_calibration.runtime.preparation_witness import _insufficient_rotation_before_fit + from linkerhand_calibration.core.geometry.tag_pose import motion_image_diagnostics as diagnostics + monkeypatch.setattr(diagnostics, '_validate_frames', lambda *a: ({'tag': ((None,),)}, ('tag',))) + assert not _insufficient_rotation_before_fit((), None) + + +def test_declined_auxiliary_does_not_overrule_ambiguous_main_images(real_capture, monkeypatch): + from linkerhand_calibration.runtime import branch_initialization as module + from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution + _, _, request = real_capture + monkeypatch.setattr(module, 'resolve_image_motion', lambda *a, **k: + ImageMotionResolution(False, 'image_motion_families_not_distinguishable')) + result = solve_initialization(request) + assert not result.resolved and result.reason == 'image_motion_families_not_distinguishable' + + +def test_auxiliary_fit_failure_cannot_choose_another_acceptance_route(real_capture, monkeypatch): + from linkerhand_calibration.runtime import branch_initialization as main, preparation_witness as auxiliary + from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution + _, _, request = real_capture + monkeypatch.setattr(auxiliary, '_insufficient_rotation_before_fit', lambda *a: False) + monkeypatch.setattr(auxiliary, 'resolve_image_motion', lambda *a, **k: + ImageMotionResolution(False, 'image_motion_families_not_distinguishable')) + monkeypatch.setattr(main, 'resolve_image_motion', lambda *a, **k: pytest.fail('no fallback after auxiliary fit')) + result = solve_initialization(request) + assert not result.resolved and result.reason.startswith('preparation_witness_unobservable:') + + +def test_excessive_sync_loss_has_its_own_category_without_motion_retry(): + from linkerhand_calibration.runtime.zero_recovery import failure_category, FailureCategory, permits_recovery + from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh + for reason in ('preparation_sync_coverage_missing', 'preparation_sync_endpoint_missing', + 'preparation_sync_gap_too_long', 'preparation_state_vector_invalid'): + assert failure_category(reason) == FailureCategory.EVIDENCE_GAP + assert not permits_recovery([reason], 0) + assert reason_zh({'reason': 'joint_zero_motion_unresolved:'+reason}, model_name='O30')[0] == 'OBS-PREP-SYNC-119' + + +@pytest.mark.replay +@pytest.mark.parametrize('fault', ['erase_missing', 'change_rejection', 'change_reason', 'remove_record']) +def test_raw_gaps_and_admission_decision_remain_auditable(real_capture, replayed, fault): + profile, _, _ = real_capture + _, original = replayed + records = deepcopy(original) + report = records[-1] + if fault == 'erase_missing': + stamp = report['preparation_witnesses'][0]['sample_admission']['rejected_frames'][0]['image_stamp_ns'] + records = [r for r in records if r.get('image_stamp_ns') != stamp] + else: + for container in [report, *report['evidence_payloads'].values()]: + if fault == 'remove_record': + container.pop('preparation_witnesses') + elif fault == 'change_rejection': + container['preparation_witnesses'][0]['sample_admission']['rejected_frames'] = [] + else: + container['preparation_witnesses'][0]['reason'] = 'ignore_bad_fit' + if fault == 'remove_record': + report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' + for role, payload in report['evidence_payloads'].items(): + report['evidence_ids'][role] = source_frame_sha256(payload) + with pytest.raises(ValueError): + evidence = _initializations(records) + validate_preparation_witnesses(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_preparation_transition.py b/src/linkerhand_calibration/test/test_preparation_transition.py new file mode 100644 index 0000000..67bcce5 --- /dev/null +++ b/src/linkerhand_calibration/test/test_preparation_transition.py @@ -0,0 +1,216 @@ +"""The real 2026-09-20 MCP pause, through runtime admission and journal audit.""" + +from copy import deepcopy +from dataclasses import replace +import gzip +import json +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.domain.reference import JointZeroReference +from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256, validate_source_geometry +from linkerhand_calibration.runtime.passed_resume import restore_parent_models +from linkerhand_calibration.runtime.preparation_transition import build_preparation_transition +from linkerhand_calibration.runtime.preparation_transition_evidence import validate_preparation_transitions +from o30_capture_fixture import SOURCE + +pytestmark = pytest.mark.replay + + +@pytest.fixture(scope='module') +def recording(): + path = Path(__file__).parent/'fixtures/o30_thumb_preparation_transition.json.gz' + return json.loads(gzip.decompress(path.read_bytes())) + + +def request_from_recording(recording): + profile = load_bundled_hand_profile('o30_right_18') + owner = BranchInitialization(profile, source_urdf=SOURCE) + reference = JointZeroReference.from_record(next(r for r in recording['records'] if r['kind'] == 'joint_zero_reference')) + report = next(r for r in recording['records'] if r['kind'] == 'motion_branch_initialization') + restore_parent_models(owner.parent_references, [report], 1) + owner.parent_references.remember_zero(reference, 1) + spec = profile.motion.joint_zero_references['thumb_mcp'] + motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('thumb_mcp',)) + for version in (46, 48): + owner.begin(motion, session_epoch=1, motion_version=version) + for row in recording['records']: + if row['kind'] == 'motion_branch_observation' and row['task_name'] == spec.task_key and row['motion_version'] == version: + owner.observe(row) + request = owner.request('front', replace(motion, phase='joint_zero', zero_joints=('thumb_mcp',)), + session_epoch=1, motion_version=49, zero_references={reference.joint: reference}) + return owner, request + + +@pytest.fixture(scope='module') +def replayed(recording, tmp_path_factory): + owner, request = request_from_recording(recording) + assert not request.invalid_reason and request.transition_source is not None + result = solve_initialization(request) + assert result.resolved, result.reason + _, report = initialization_artifacts(request, result) + (tmp_path_factory.mktemp('preparation_transition')/'replay_report.json').write_text( + json.dumps(report, indent=2, allow_nan=False)) + records = [*recording['records'], report] + return owner.profile, request, result, records + + +def test_real_mcp_requires_transition_evidence_not_a_lower_error_winner(replayed): + _, request, result, _ = replayed + old = resolve_image_motion(request.image_frames, request.relations) + assert not old.resolved and old.reason == 'image_motion_families_not_distinguishable' + assert len(request.frames) == 128 and len(request.transition_source.rows) == 138 + assert len(result.hypotheses) == 4 + accepted = [h for h in result.hypotheses if not h.reason] + assert len(accepted) == 2 + assert result.model.maximum_reprojection_error_px == 1.5 + assert result.model.reprojection_tie_px == .03 + assert all(q.maximum_frame_rms_px < 1.5 for h in accepted + for q in (*h.training_quality, *h.validation_quality)) + assert all(h.reason == 'image_motion_reprojection_failed' for h in result.hypotheses if h.reason) + assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) + assert result.preparation_transitions[0]['source_frame_hashes'] == { + str(r['image_stamp_ns']): source_frame_sha256(r) for r in request.transition_source.rows} + + +def test_real_transition_rebuilds_through_formal_evidence_boundary(replayed): + profile, _, _, records = replayed + evidence = _initializations(records) + validate_source_geometry(profile, SOURCE, records) + validate_preparation_transitions(profile, records, evidence) + + +@pytest.mark.parametrize('fault', ['held_feedback', 'held_command', 'feedback_stuck', 'endpoint', + 'camera', 'epoch', 'validation', 'duplicate', 'installation']) +def test_invalid_entry_evidence_cannot_authorize_a_model(replayed, fault): + profile, request, _, _ = replayed + rows = deepcopy(request.transition_source.rows) + if fault == 'held_feedback': + rows[25]['feedback_vector'][6] += 8 + elif fault == 'held_command': + rows[25]['command_vector'][6] = 1 + elif fault == 'feedback_stuck': + for row in rows: + row['feedback_vector'][1] = 0 + elif fault == 'endpoint': + rows = tuple(r for r in rows if r['command_vector'][1] != 80) + tuple( + r for r in rows if r['command_vector'][1] == 80)[:9] + elif fault == 'camera': + for row in rows: + row['camera_matrix'][0][0] += 1 + elif fault == 'epoch': + rows[25]['session_epoch'] += 1 + elif fault == 'validation': + rows[25]['cycle'] = 3 + elif fault == 'duplicate': + rows[25]['image_stamp_ns'] = rows[24]['image_stamp_ns'] + else: + for row in rows[:7]: + row['tags']['thumb_mcp']['corners_xy'][0][0] += 30 + source = replace(request.transition_source, rows=tuple(rows)) + with pytest.raises(ValueError): + build_preparation_transition(profile, source, request.current_source_rows) + + +@pytest.mark.parametrize('fault', ['missing_pixels', 'policy', 'winner', 'source_zero', 'omitted_frame']) +def test_tampered_evidence_is_rejected_before_publication(replayed, fault): + _, _, _, records = replayed + records = deepcopy(records) + report = records[-1] + source_stamp = int(next(iter(report['preparation_transitions'][0]['source_frame_hashes']))) + if fault == 'missing_pixels': + records[:] = [r for r in records if not (r.get('kind') == 'motion_branch_observation' and r.get('image_stamp_ns') == source_stamp)] + elif fault == 'policy': + report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' + elif fault == 'winner': + report['selected_hypothesis'] = 1 + elif fault == 'source_zero': + records[:] = [r for r in records if r.get('kind') != 'joint_zero_reference'] + else: + for payload in [report, *report['evidence_payloads'].values()]: + payload['preparation_transitions'][0]['source_frame_hashes'].pop(str(source_stamp), None) + for role, payload in report['evidence_payloads'].items(): + report['evidence_ids'][role] = source_frame_sha256(payload) + with pytest.raises(ValueError): + _initializations(records) + + +def test_cancellation_and_new_epoch_do_not_reuse_partial_transition(recording, replayed): + owner, request = request_from_recording(recording) + owner.clear() + assert owner.accept(request, replayed[2]) is None + assert not owner.transition_capture.rows + motion = MotionCommand('zero_approach', owner.profile.motion.joint_zero_references['thumb_mcp'].command, + 200., task_key='thumb_mcp_front', reference_joints=('thumb_mcp',)) + owner.begin(motion, session_epoch=2, motion_version=1) + assert owner.transition_capture.source is None + + +def test_transition_holdout_only_vetoes_without_retraining_or_selecting_another_family(replayed): + profile, request, original, _ = replayed + transition, _ = build_preparation_transition(profile, request.transition_source, request.current_source_rows) + witness = next(frame.evidence.stamp_ns for i, frame in enumerate(transition.frames) + if i % 2 == 1 and transition.endpoints[i] == -1) + rows = deepcopy(request.transition_source.rows) + row = next(r for r in rows if r['image_stamp_ns'] == witness) + for corner in row['tags']['thumb_mcp']['corners_xy']: + corner[0] += 40 + result = solve_initialization(replace(request, transition_source=replace(request.transition_source, rows=rows))) + assert not result.resolved + assert [h.training_quality for h in result.hypotheses] == [h.training_quality for h in original.hypotheses] + + +def test_accepted_transition_allows_runtime_to_continue(recording, replayed): + owner, request = request_from_recording(recording) + assert owner.accept(request, replayed[2]) is not None + spec = owner.profile.motion.joint_zero_references['thumb_mcp'] + motion = MotionCommand('joint_zero', spec.command, 200., task_key=spec.task_key, + zero_joints=('thumb_mcp',)) + assert owner.ready(motion) + assert owner.error() == '' + + +@pytest.mark.parametrize('independently_observable', [False, True]) +def test_missing_optional_transition_uses_original_observability_gate(replayed, independently_observable): + _, request, _, _ = replayed + request = replace(request, transition_source=replace(request.transition_source, + rows=request.transition_source.rows[:20])) + if independently_observable: + from test_motion_image_diagnostics import frames, RELATIONS + request = replace(request, image_frames=frames(), relations=RELATIONS) + result = solve_initialization(request) + assert result.resolved == independently_observable + assert result.preparation_transition_rejections == ('preparation_transition_samples_missing',) + assert not result.preparation_transitions + if not independently_observable: + assert result.reason == 'image_motion_families_not_distinguishable' + + +def test_changed_dependency_mask_cannot_hide_a_moving_joint(replayed): + profile, _, _, records = replayed + records = deepcopy(records) + report = records[-1] + for payload in [report, *report['evidence_payloads'].values()]: + payload['preparation_transitions'][0]['relevant_channels'] = [1] + for role, payload in report['evidence_payloads'].items(): + report['evidence_ids'][role] = source_frame_sha256(payload) + with pytest.raises(ValueError, match='preparation_transition_topology_changed'): + validate_source_geometry(profile, SOURCE, records) + + +def test_changed_frozen_model_cannot_replace_original_image_solution(replayed): + profile, _, _, records = replayed + evidence = deepcopy(_initializations(records)) + for payload in evidence.values(): + if payload.get('preparation_transitions'): + # Recomputing the outer digest cannot replace raw-image replay. + geometry = payload['frozen_image_model']['geometry'][0] + geometry['mount_child_xyz_m'] = tuple(v+.001 for v in geometry['mount_child_xyz_m']) + payload['image_model_sha256'] = source_frame_sha256(payload['frozen_image_model']) + with pytest.raises(ValueError, match='preparation_transition_frozen_model_changed'): + validate_preparation_transitions(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_preparation_witness.py b/src/linkerhand_calibration/test/test_preparation_witness.py new file mode 100644 index 0000000..2a55004 --- /dev/null +++ b/src/linkerhand_calibration/test/test_preparation_witness.py @@ -0,0 +1,343 @@ +"""Real ambiguity stays rejected; independent synthetic views exercise the new path.""" +from copy import deepcopy +from dataclasses import asdict, replace +import gzip +import json +from pathlib import Path + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.preparation import configure_preparation +from linkerhand_calibration.core.domain.capture_plan import configure_training, CapturePlan +from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver +from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame +from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe +from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.preparation_witness import WitnessInput, build_preparation_witness, select_witness_rows +from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 +from linkerhand_calibration.runtime.preparation_witness_evidence import validate_preparation_witnesses, validate_witness_topology +from o30_capture_fixture import SOURCE + + +@pytest.fixture(scope='module') +def ring_source(): + data = json.loads(gzip.decompress(Path(__file__).with_name('fixtures').joinpath('o30_ring_mcp_ambiguous.json.gz').read_bytes())) + profile = load_bundled_hand_profile('o30_right_18') + owner = BranchInitialization(profile, source_urdf=SOURCE) + failure = data['failure'] + spec = profile.motion.joint_zero_references['ring_mcp_pitch'] + motion = MotionCommand('zero_approach', spec.command, 200., task_key=failure['task_name'], reference_joints=('ring_mcp_pitch',)) + owner.begin(motion, session_epoch=1, motion_version=failure['motion_version']) + for row in data['rows']: + owner.observe(row) + request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('ring_mcp_pitch',)), + session_epoch=1, motion_version=failure['motion_version']+1) + fits = [] + original = solver._fit_hypothesis + def trace(*args, **kwargs): + fit = original(*args, **kwargs) + fits.append(fit) + return fit + with pytest.MonkeyPatch.context() as patch: + patch.setattr(solver, '_fit_hypothesis', trace) + result = solve_initialization(request) + return profile, owner, request, result, tuple(fits), tuple(data['rows']) + + +@pytest.mark.replay +def test_real_ring_is_not_authorized_without_new_pose_information(ring_source): + _, _, request, result, _, _ = ring_source + assert len(request.image_frames) == 127 + assert not result.resolved and result.reason == 'image_motion_families_not_distinguishable' + assert all(h.training_accepted for h in result.hypotheses) + + +@pytest.fixture(scope='module') +def witnessed_ring(ring_source): + old_profile, owner, request, _, fits, rows = ring_source + profile = configure_preparation(configure_training(old_profile, 'staged_2_to_3'), 'occlusion_aware_witness_v1') + channels = tuple(sorted(owner.tag_feedback_channels['ring_pip'])) + arc_stamps = {f.evidence.stamp_ns for f in request.image_frames} + endpoints = tuple(r for r in owner._endpoint_records['side'] if r['image_stamp_ns'] not in arc_stamps) + # This is a *synthetic* independent yaw arc, anchored to candidate 0 to + # exercise selection. It does not claim that real hardware chose branch 0. + model = fits[0][1] + pose = next(s.pose for s in select_image_motion_frame(model, request.image_frames[-1]).selections if s.role == 'ring_pip') + orientation = Rotation.from_quat(pose.quaternion_xyzw) + point = np.asarray(pose.translation_xyz_m) + pivot = point-np.array([0., .03, -.04]) + donor = profile.motion.preparation_witnesses['ring_mcp_pitch'] + target_spec = profile.motion.joint_zero_references['ring_mcp_pitch'] + start_command, excursion = donor.command[4], donor.approach_commands[1][4] + segments = [(start_command, excursion), (excursion, start_command)] + start = request.image_frames[0].evidence.stamp_ns-20_000_000_000 + sources = [] + for path, (before, after) in enumerate(segments, 1): + values = list(np.linspace(before, after, 33)) + if path == len(segments): + values += [after]*11 + for value in values: + row = deepcopy(rows[0]); stamp = start+len(sources)*40_000_000 + row.update(task_name=target_spec.task_key, zero_joints=['ring_mcp_pitch'], + preparation_witness_joint='ring_mcp_roll', + motion_version=100+path, reference_path_index=path, image_stamp_ns=stamp, + command_vector=list(donor.command), feedback_vector=list(donor.command)) + row['command_vector'][4] = row['feedback_vector'][4] = float(value) + turn = Rotation.from_rotvec(np.array([0., 1., 0.])*np.deg2rad((value-113)*.3)) + rotation, translation = turn*orientation, pivot+turn.apply(point-pivot) + points = rotation.apply(square_object_points(.0165))+translation + image = points@np.asarray(row['camera_matrix']).T + corners = image[:, :2]/image[:, 2:] + tag = row['tags']['ring_pip'] + tag['corners_xy'] = corners.tolist() + tag['selected_pose'] = None + tag['reprojection_valid_candidates'] = [asdict(p) for p in solve_square_tag_ippe(corners, + tag_size_m=.0165, camera_matrix=row['camera_matrix'])] + row['tags'] = {r: row['tags'][r] for r in ('side_base', 'ring_pip')} + for item in row['tags'].values(): + item['candidate_diagnostics']['observation_stamp_ns'] = stamp + sources.append(row) + source = WitnessInput('ring_mcp_pitch', 'ring_mcp_roll', tuple(sources), endpoints, channels) + bridge, record = build_preparation_witness(profile, source) + result = solver.resolve_image_motion(request.image_frames, request.relations, pose_bridges=(bridge,)) + assert result.resolved, result.reason + request = replace(request, witness_sources=(source,), witness_profile=profile) + result = replace(result, preparation_witnesses=(record,)) + _, report = initialization_artifacts(request, result) + # Journal round trip makes tuple/list normalization part of this test. + records = json.loads(json.dumps([*sources, *rows, report])) + return profile, source, bridge, record, request, result, records + + +def test_independent_witness_resolves_real_recipient_without_changing_limits(witnessed_ring): + profile, source, bridge, record, request, result, _ = witnessed_ring + assert result.resolved and len(result.hypotheses) == 2 + assert sum(h.training_accepted for h in result.hypotheses) == 1 + assert result.model.maximum_reprojection_error_px == 1.5 and result.model.reprojection_tie_px == .03 + assert bridge.rotation_uncertainty_rad > 0 and bridge.translation_uncertainty_m > 0 + assert set(record['source_model']['training_stamps_ns']).isdisjoint(record['source_model']['validation_stamps_ns']) + assert set(record['source_frame_hashes']).isdisjoint(record['endpoint_frame_hashes']) + assert set(record['endpoint_frame_hashes']).isdisjoint(dict(request.source_frame_hashes)) + assert source.rows[-1]['image_stamp_ns'] < request.image_frames[0].evidence.stamp_ns + + +def test_witness_formal_evidence_replays_original_pixels_and_model(witnessed_ring): + from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel + profile, _, _, _, _, _, records = witnessed_ring + evidence = _initializations(records) + validate_witness_topology(profile, records, evidence, UrdfKinematicModel(SOURCE)) + validate_preparation_witnesses(profile, records, evidence) + + +@pytest.mark.legacy +def test_legacy_witness_evidence_keeps_its_original_admission_contract(witnessed_ring): + profile, source, _, _, request, result, records = witnessed_ring + _, legacy = build_preparation_witness(profile, source, legacy=True) + assert legacy['version'] == 'preparation_witness_v1' and 'sample_admission' not in legacy + for hypothesis in legacy['source_hypotheses']: + hypothesis.pop('training_solver_attempts', None) + _, report = initialization_artifacts(request, replace(result, preparation_witnesses=(legacy,))) + records = json.loads(json.dumps([*records[:-1], report])) + evidence = _initializations(records) + validate_preparation_witnesses(profile, records, evidence) + + +@pytest.mark.parametrize('change', ['pixels', 'missing', 'policy', 'window']) +def test_witness_tampering_is_rejected_without_whole_hand_refitting(witnessed_ring, change): + _, _, _, _, _, _, original = witnessed_ring + records = deepcopy(original) + if change == 'pixels': + records[1]['tags']['ring_pip']['corners_xy'][0][0] += .1 + elif change == 'missing': + del records[1] + elif change == 'policy': + records[-1]['image_model_selection_policy'] = 'training_squared_loss_holm_v1' + else: + payload = next(iter(records[-1]['evidence_payloads'].values())) + # Even coordinated hash updates must not hide an inconvenient raw frame. + record = records[-1]['preparation_witnesses'][0] + stamp = sorted(record['source_frame_hashes'], key=int)[1] + del record['source_frame_hashes'][stamp] + payload['preparation_witnesses'] = deepcopy(records[-1]['preparation_witnesses']) + role = payload['tag_role']; records[-1]['evidence_ids'][role] = source_frame_sha256(payload) + with pytest.raises(ValueError): + _initializations(records) + + +@pytest.mark.parametrize('change', ['command', 'feedback', 'formal', 'endpoint', 'missing']) +def test_invalid_witness_never_triggers_silent_fallback(witnessed_ring, change): + profile, source, _, _, request, _, _ = witnessed_ring + rows = deepcopy(source.rows) + endpoints = deepcopy(source.endpoint_rows) + if change == 'command': + rows[3]['command_vector'][9] += 1 + elif change == 'feedback': + rows[3]['feedback_vector'][9] += 3 + elif change == 'formal': + rows[3]['cycle'] = 3 + elif change == 'endpoint': + endpoints[0]['feedback_vector'][4] += 11 + else: + rows = () + changed = replace(source, rows=tuple(rows), endpoint_rows=tuple(endpoints)) + result = solve_initialization(replace(request, witness_sources=(changed,))) + assert not result.resolved and result.reason.startswith('preparation_witness_') + + +def test_plan_changes_are_opt_in_and_preserve_existing_tag_and_motion_contracts(): + from linkerhand_calibration.profiles.validator import validate_executable_profile + old = configure_training(load_bundled_hand_profile('o30_right_18'), 'staged_2_to_3') + new = configure_preparation(old, 'occlusion_aware_witness_v1') + assert new.vision == old.vision and new.quality == old.quality + assert {t.key: t for t in new.motion.tasks} == {t.key: t for t in old.motion.tasks} + assert new.motion.joint_zero_references == old.motion.joint_zero_references + assert CapturePlan.from_profile(new).sha256 != CapturePlan.from_profile(old).sha256 + assert 'preparation_witnesses' not in CapturePlan.from_profile(old).as_dict()['motion_contract']['motion'] + validate_executable_profile(new, SOURCE) + + +def test_live_collector_and_request_use_same_witness_as_offline(witnessed_ring, ring_source): + profile, source, bridge, _, _, expected, _ = witnessed_ring + _, _, old_request, _, _, rows = ring_source + owner = BranchInitialization(profile, source_urdf=SOURCE) + # The donor's optional side view never becomes a formal roll measurement. + for row in source.rows: + owner.observe(row) + assert owner._frames == {} + target = profile.motion.joint_zero_references[source.joint] + motion = MotionCommand('zero_approach', target.command, 200., task_key=target.task_key, + reference_joints=(source.joint,)) + owner.begin(motion, session_epoch=1, motion_version=old_request.motion_version) + for row in rows: + owner.observe(row) + request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=(source.joint,)), + session_epoch=1, motion_version=old_request.motion_version+1) + assert len(request.witness_sources) == 1 + result = solve_initialization(request) + # The live budget now retains more recipient frames. The independent + # witness stays identical; reproduce the recipient with that exact budget. + reproduced = solver.resolve_image_motion(request.image_frames, request.relations, + pose_bridges=(bridge,), parameters=request.image_parameters) + assert result.resolved and result.model == reproduced.model + assert result.preparation_witnesses == expected.preparation_witnesses + model, report = owner.accept(request, result) + assert model is not None and report['preparation_witnesses'] + + +def test_collector_is_bounded_and_epoch_reset_does_not_relabel_history(witnessed_ring): + from linkerhand_calibration.runtime.preparation_witness import MAXIMUM_WINDOW_FRAMES + profile, source, *_ = witnessed_ring + owner = BranchInitialization(profile, source_urdf=SOURCE) + row = source.rows[0] + for _ in range(MAXIMUM_WINDOW_FRAMES+20): + owner.witness_capture.observe(row) + assert len(next(iter(owner.witness_capture.windows.values()))) == MAXIMUM_WINDOW_FRAMES+1 + owner.witness_capture.observe(dict(row, session_epoch=2)) + assert list(owner.witness_capture.windows) == [(2, row['task_name'], row['view'])] + assert len(next(iter(owner.witness_capture.windows.values()))) == 1 + + +def test_missing_optional_tag_does_not_block_donor_capture(witnessed_ring): + from linkerhand_calibration.runtime.preparation_witness import witness_roles + profile, source, *_ = witnessed_ring + owner = BranchInitialization(profile, source_urdf=SOURCE) + motion = MotionCommand('zero_approach', profile.motion.joint_zero_references[source.donor].command, + 200., task_key='fingers_mcp_roll_front', reference_joints=(source.donor,)) + motion = replace(motion, reference_joints=('ring_mcp_pitch',), preparation_witness_joint='ring_mcp_roll') + assert witness_roles(profile, motion) == {'side_base', 'ring_pip', 'front_base', 'ring_mcp_roll'} + row = deepcopy(source.rows[0]); row['tags'].pop('ring_pip') + owner.observe(row) + assert not owner._invalid_reason + assert owner._frames == {} + + +def test_reference_change_and_unobservability_cannot_trigger_same_arc_retry(): + from linkerhand_calibration.runtime.zero_recovery import failure_category, permits_recovery, FailureCategory + assert failure_category('preparation_witness_unobservable:ambiguous') == FailureCategory.UNOBSERVABLE + assert failure_category('preparation_witness_reference_feedback_changed') == FailureCategory.REFERENCE_CHANGED + assert not permits_recovery(('preparation_witness_unobservable:ambiguous',), 0) + + +@pytest.mark.parametrize('change', ['holdout', 'installation']) +def test_new_images_can_veto_but_cannot_replace_witness(witnessed_ring, change): + profile, source, _, record, request, _, _ = witnessed_ring + rows, endpoints = deepcopy(source.rows), deepcopy(source.endpoint_rows) + if change == 'holdout': + stamp = record['source_model']['validation_stamps_ns'][4] + affected = [r for r in rows if r['image_stamp_ns'] == stamp] + else: + affected = endpoints + for row in affected: + row['tags']['ring_pip']['corners_xy'] = [[x+10, y+8] for x, y in row['tags']['ring_pip']['corners_xy']] + result = solve_initialization(replace(request, witness_sources=(replace(source, + rows=tuple(rows), endpoint_rows=tuple(endpoints)),))) + assert not result.resolved and result.reason.startswith('preparation_witness_') + assert result.model is None + + +def test_required_witness_cannot_be_removed_by_updating_hashes(witnessed_ring): + profile, _, _, _, _, _, original = witnessed_ring + records = deepcopy(original) + report = records[-1] + del report['preparation_witnesses'] + report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' + for role, payload in report['evidence_payloads'].items(): + del payload['preparation_witnesses'] + report['evidence_ids'][role] = source_frame_sha256(payload) + evidence = _initializations(records) + with pytest.raises(ValueError, match='required_evidence_missing'): + validate_preparation_witnesses(profile, records, evidence) + + +def test_witness_configuration_round_trip_and_topology_are_explicit(tmp_path): + from linkerhand_calibration.profiles.export import write_hand_profile + from linkerhand_calibration.profiles import load_hand_profile + from linkerhand_calibration.profiles.preparation import validate_preparation_witnesses + from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel + profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), + 'staged_2_to_3'), 'occlusion_aware_witness_v1') + path = tmp_path/'witness.yaml'; write_hand_profile(profile, path) + restored = load_hand_profile(path) + assert CapturePlan.from_profile(restored) == CapturePlan.from_profile(profile) + invalid = replace(profile, motion=replace(profile.motion, + preparation_witnesses={'ring_mcp_pitch': replace(profile.motion.preparation_witnesses['ring_mcp_pitch'], joint='pinky_mcp_roll')})) + with pytest.raises(ValueError, match='posture_not_shared'): + validate_preparation_witnesses(invalid, UrdfKinematicModel(SOURCE)) + + +@pytest.mark.parametrize('joint', ['pinky_mcp_pitch', 'ring_mcp_pitch', 'middle_mcp_pitch', 'index_mcp_pitch']) +def test_occluded_fingers_get_one_local_excitation_after_clearance(joint): + from linkerhand_calibration.runtime.execution import SessionExecution + from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint + profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), + 'staged_2_to_3'), 'occlusion_aware_witness_v1') + driver = SessionExecution(profile) + task_key = profile.motion.joint_zero_references[joint].task_key + driver.session._unit_index = next(i for i, unit in enumerate(driver.session._units) if unit.task_key == task_key) + driver.session.phase = Phase.PREPARE + effects = driver._make_effects(profile.command.baseline_values) + witness = profile.motion.preparation_witnesses[joint] + excitation = [m for m in effects if m.preparation_witness_joint] + assert len(excitation) == 3 and [m.reference_path_index for m in excitation] == [0, 1, 2] + first = effects.index(excitation[0]) + assert first > 0 and effects[first-1].phase == 'clearance' + assert effects[first-1].target == witness.command == excitation[-1].target + channel = channel_for_joint(profile, witness.joint) + for motion in excitation: + assert not motion.recording and motion.command_index == channel + assert all(v == int(v) for v in motion.target) + assert all(v == witness.command[i] for i, v in enumerate(motion.target) if i != channel) + normal = next(i for i, m in enumerate(effects) if m.phase == 'zero_approach' and not m.preparation_witness_joint) + assert effects.index(excitation[-1]) < normal + # Revisit and independent validation do not re-excite or refit a frozen zero. + driver = SessionExecution(profile) + driver.zero_references[joint] = object() + driver.session._unit_index = next(i for i, unit in enumerate(driver.session._units) + if unit.task_key == task_key and unit.cycle == 1) + driver.session.phase = Phase.PREPARE + assert not any(m.preparation_witness_joint for m in driver._make_effects(witness.command)) diff --git a/src/linkerhand_calibration/test/test_prepared_resume.py b/src/linkerhand_calibration/test/test_prepared_resume.py index bc797a1..f6e5465 100644 --- a/src/linkerhand_calibration/test/test_prepared_resume.py +++ b/src/linkerhand_calibration/test/test_prepared_resume.py @@ -15,10 +15,16 @@ from runtime_host_fixture import coordinator_fixture, ready @pytest.mark.parametrize("changed_reference", [False, True]) @pytest.mark.parametrize("model,layout", [("o6", "o6_right_8"), ("l6", "l6_right_8"), - ("o12", "o12_right_16"), ("g20", "g20_right_19")]) + ("o12", "o12_right_16"), ("g20", "g20_right_19"), ("o30", "o30_right_18")]) def test_resume_tick_uses_prepared_data_and_still_checks_current_reference( tmp_path, monkeypatch, model, layout, changed_reference): profile = load_bundled_hand_profile(layout) + transform = None + if model == "o30": + from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training + from linkerhand_calibration.runtime import staged_resume + transform = lambda p: configure_training(p, STAGED) + profile = transform(profile) fingerprint = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "protected_hashes": {"source": "a" * 64}, "fixed_corners_by_view": {v: [[10, 10], [20, 10], [20, 20], [10, 20]] for v in profile.vision.view_names}, @@ -28,16 +34,37 @@ def test_resume_tick_uses_prepared_data_and_still_checks_current_reference( "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version}, fingerprint] + if model == "o30": + rows[0]["capture_plan"] = CapturePlan.from_profile(profile).as_dict() path = tmp_path / "previous.jsonl" path.write_text("\n".join(json.dumps(row) for row in rows)) staged = [] # Actual provenance and per-unit quality replay are covered by their own # recorded-data tests; here assert that this expensive boundary is pre-live. monkeypatch.setattr(JointResume, "stage", lambda self, *args: staged.append(self)) - host, clock = coordinator_fixture(tmp_path, model=model, resume_raw_samples_path=path) + prepared_imports = [] + if model == "o30": + original_import = staged_resume.reference_import + def prepare_visits(resume, engine, records): + resume.rows = tuple(records) + staged.append(resume) + def prepare_import(resume): + pending = original_import(resume) + prepared_imports.append(pending) + return pending + monkeypatch.setattr(staged_resume, "stage_visits", prepare_visits) + monkeypatch.setattr(staged_resume, "reference_import", prepare_import) + host, clock = coordinator_fixture(tmp_path, model=model, resume_raw_samples_path=path, + profile_transform=transform) try: assert len(staged) == 1 and not host.started and not clock.positions assert host.joint_resume is not staged[0] + if model == "o30": + assert len(prepared_imports) == 1 + assert host._resume_checkpoint.passed_import is prepared_imports[0] + assert host._passed_reference_import is None + monkeypatch.setattr(staged_resume, "reference_import", + lambda *args: pytest.fail("historical provenance indexing in live callback")) path.unlink() # The immutable loaded snapshot is sufficient after startup. monkeypatch.setattr(JointResume, "stage", lambda *args: pytest.fail("geometry replay in live callback")) ready(host, clock) @@ -50,7 +77,9 @@ def test_resume_tick_uses_prepared_data_and_still_checks_current_reference( host._current_fingerprint["fixed_corners_by_view"][view][0][0] += 20 host.execution.session.phase = Phase.RESUME_VERIFY host.tick() - assert host.execution.session.phase == Phase.PREPARE + assert host.execution.session.phase == (Phase.PAUSED if model == "o30" and changed_reference else Phase.PREPARE) + if model == "o30": + assert (host._passed_reference_import is prepared_imports[0]) is not changed_reference assert host.feedback_fresh() assert (host.joint_resume is staged[0]) is not changed_reference assert host._resumed_count == 0 # Each old zero still needs physical verification. @@ -128,7 +157,7 @@ def test_large_task_import_yields_to_feedback_and_commits_only_after_fsync( @pytest.mark.parametrize("interruption", [None, "abort", "feedback_loss"]) def test_zero_provenance_import_yields_and_never_authorizes_an_incomplete_zero( tmp_path, monkeypatch, interruption): - from types import SimpleNamespace + from linkerhand_calibration.runtime.motion_execution import MotionCommand from linkerhand_calibration.runtime import coordinator from test_motion_reference_provenance import motion_capture @@ -138,7 +167,8 @@ def test_zero_provenance_import_yields_and_never_authorizes_an_incomplete_zero( assert host.start().success host.execution.session.phase = Phase.PREPARE _, reference, _ = motion_capture(layout="o6_right_8") - motion = SimpleNamespace(zero_joints=(reference.joint,)) + motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 1., + task_key=reference.task_key, zero_joints=(reference.joint,)) # Geometry and old/new pose comparisons have their own independent # provenance tests. Exercise the real coordinator's transfer boundary. monkeypatch.setattr(host.branch_initialization, "ready", lambda _: True) diff --git a/src/linkerhand_calibration/test/test_shared_pose_handoff.py b/src/linkerhand_calibration/test/test_shared_pose_handoff.py new file mode 100644 index 0000000..4e7b42f --- /dev/null +++ b/src/linkerhand_calibration/test/test_shared_pose_handoff.py @@ -0,0 +1,282 @@ +"""Real MCP -> PIP evidence handoff, with immutable zeros and legacy replay.""" +from copy import deepcopy +from dataclasses import replace +import gzip +import json +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.domain.capture_plan import configure_training +from linkerhand_calibration.core.domain.reference import JointZeroReference +from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import ( + check_source_pose, check_pose_bridge, SHARED_TAG_HANDOFF_POLICY, +) +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.preparation import configure_preparation +from linkerhand_calibration.runtime.branch_initialization import ( + BranchInitialization, solve_initialization, initialization_artifacts, +) +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 +from linkerhand_calibration.runtime.passed_resume import restore_parent_models +from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references +from linkerhand_calibration.runtime.shared_pose_handoff import source_window +from o30_capture_fixture import SOURCE + + +@pytest.fixture(scope='module') +def capture(): + data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_pinky_handoff.json.gz').read_bytes())) + profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), + 'staged_2_to_3'), 'occlusion_aware_witness_v1') + owner = BranchInitialization(profile, source_urdf=SOURCE) + failure = data['original_failure'] + reference = JointZeroReference.from_record(failure['shared_tag_pose_bridges'][0]['source_reference']) + source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') + restore_parent_models(owner.parent_references, [source], 1) + motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['pinky_pip'].command, + 200., task_key='pinky_pip_side', reference_joints=('pinky_pip',)) + owner.begin(motion, session_epoch=1, motion_version=failure['motion_version']) + for row in data['records']: + if row['kind'] == 'motion_branch_observation': + owner.observe(row) + owner.parent_references.handoff.observe(row) + request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('pinky_pip',)), + session_epoch=1, motion_version=failure['motion_version']+1, zero_references={reference.joint:reference}) + assert not request.invalid_reason + return profile, owner, request, data, reference + + +@pytest.fixture(scope='module') +def replay(capture): + profile, owner, request, data, reference = capture + result = solve_initialization(request) + assert result.resolved, result.reason + model, report = initialization_artifacts(request, result) + return result, model, report, [*data['records'], report] + + +@pytest.mark.replay +def test_real_handoff_preserves_gates_and_formal_readback(capture, replay): + profile, _, request, _, reference = capture + result, _, report, records = replay + bridge = request.pose_bridges[0] + assert check_source_pose(replace(bridge, source_frames=())).reason.startswith( + 'shared_pose_current_image_unresolved:image_motion_frame_reprojection_or_tilt_failed') + assert check_source_pose(bridge).status == 'consistent' + assert check_pose_bridge(result.model, bridge).status == 'consistent' + assert len(bridge.source_frames) == len(bridge.frames) == 10 + assert max(f.evidence.stamp_ns for f in bridge.source_frames) < min(f.evidence.stamp_ns for f in request.image_frames) + assert report['image_model_selection_policy'] == SHARED_TAG_HANDOFF_POLICY + assert report['shared_tag_pose_bridges'][0]['source_reference'] == reference.as_record() + assert result.model.maximum_reprojection_error_px == 1.5 + winner = result.hypotheses[result.selected_hypothesis] + assert winner.validation_quality[0].maximum_frame_rms_px < result.model.maximum_reprojection_error_px + validate_shared_tag_references(profile, records, _initializations(records)) + + +@pytest.mark.parametrize('fault', ['missing', 'feedback', 'command', 'camera', 'holdout', 'epoch', 'late', 'units', 'tag_id']) +def test_source_window_never_selects_around_bad_evidence(capture, fault): + profile, owner, request, _, reference = capture + bridge = request.pose_bridges[0] + rows = deepcopy(owner.parent_references.handoff.rows(1, reference, 'side')) + before = min(f.evidence.stamp_ns for f in request.image_frames) + if fault == 'missing': rows = rows[:-1] + elif fault == 'feedback': rows[-1]['feedback_vector'][14] += 10 + elif fault == 'command': rows[-1]['command_vector'][14] += 1 + elif fault == 'camera': rows[-1]['camera_matrix'][0][0] += 1 + elif fault == 'holdout': rows[-1]['cycle'] = 3 + elif fault == 'epoch': rows[-1]['session_epoch'] += 1 + elif fault == 'units': rows[-1]['command_unit'] = 'radian' + elif fault == 'tag_id': rows[-1]['tags']['pinky_pip']['tag_id'] = 99 + else: before = rows[-1]['image_stamp_ns'] + with pytest.raises(ValueError, match='shared_pose_handoff_'): + source_window(profile, rows, reference, (5, 10, 14), bridge.source_model, epoch=1, before_stamp=before) + + +def test_source_and_recipient_both_veto_changed_images(capture, replay): + _, _, request, _, _ = capture + bridge = request.pose_bridges[0] + def shifted(frame): + return replace(frame, observations=tuple(replace(o, + corners_xy=tuple((x+30, y) for x,y in o.corners_xy)) if o.role == 'pinky_pip' else o + for o in frame.observations)) + bad_source = replace(bridge, source_frames=tuple(map(shifted, bridge.source_frames))) + bad_target = replace(bridge, frames=tuple(map(shifted, bridge.frames))) + assert check_source_pose(bad_source).status != 'consistent' + assert check_pose_bridge(replay[0].model, bad_target).status != 'consistent' + + +def test_source_authorization_must_precede_the_entire_recipient_arc(capture): + request = capture[2] + bridge = request.pose_bridges[0] + stamp = request.image_frames[0].evidence.stamp_ns + frames = tuple(replace(f, evidence=replace(f.evidence, stamp_ns=stamp+i)) + for i, f in enumerate(bridge.source_frames)) + result = solve_initialization(replace(request, pose_bridges=(replace(bridge, source_frames=frames),))) + assert not result.resolved and result.reason.endswith('shared_pose_handoff_order_invalid') + + +@pytest.mark.parametrize('fault', ['pixels', 'remove', 'earlier_window', 'policy', 'epoch']) +def test_readback_rejects_handoff_tampering(capture, replay, fault): + profile, _, _, _, _ = capture + records = deepcopy(replay[3]) + evidence = deepcopy(_initializations(records)) + payload = next(p for p in evidence.values() if p.get('shared_tag_pose_bridges')) + bridge = payload['shared_tag_pose_bridges'][0] + stamps = bridge['source_window_hashes'] + raw = [r for r in records if r['kind'] == 'image_observation_frame'] + if fault == 'pixels': raw[-1]['tags']['pinky_pip']['corners_xy'][0][0] += 5 + elif fault == 'remove': records.remove(raw[-1]) + elif fault == 'earlier_window': + bridge['source_window_hashes'] = {str(r['image_stamp_ns']):source_frame_sha256(r) for r in raw[-11:-1]} + elif fault == 'epoch': bridge['source_window_epoch'] += 1 + else: bridge.pop('handoff_policy') + with pytest.raises(ValueError, match='shared_pose_handoff_'): + validate_shared_tag_references(profile, records, evidence) + + +def test_resume_import_retains_both_sides(capture, replay): + from linkerhand_calibration.runtime.joint_resume import _index_motion_provenance + records = list(_index_motion_provenance(replay[3]).values()) + reference = capture[4] + records.append(reference.as_record()) + assert len([r for r in records if r['kind'] == 'image_observation_frame']) == 10 + validate_shared_tag_references(capture[0], records, _initializations(records)) + + +def test_resumed_scan_and_dependency_import_share_one_exposure(capture, replay): + records = deepcopy(replay[3]) + images = [r for r in records if r['kind'] == 'image_observation_frame'] + records.extend(deepcopy(images)) + evidence = _initializations(records) + validate_shared_tag_references(capture[0], records, evidence) + records[-1]['tags']['pinky_pip']['corners_xy'][0][0] += 1 + with pytest.raises(ValueError, match='shared_pose_handoff_duplicate_image_changed'): + validate_shared_tag_references(capture[0], records, evidence) + + +def test_dip_uses_handed_off_pip_model_but_keeps_original_pose_reference(capture, replay): + profile, _, _, data, reference = capture + registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references + source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') + restore_parent_models(registry, [source, replay[2]], 1) + task = next(t for t in profile.motion.tasks if t.key == 'pinky_dip_side') + model, report = registry.pose_source(task, 1) + assert model.image_model == replay[0].model + assert report['shared_tag_pose_bridges'][0]['source_reference'] == reference.as_record() + installed = [] + from types import SimpleNamespace + registry.activate(task, 1, SimpleNamespace(activate_reference_model=installed.append)) + assert installed == [model] + altered = deepcopy(report) + altered['shared_tag_pose_bridges'][0]['source_model_sha256'] = 'f'*64 + registry.models[(1, 'side', 'pinky_pip')] = (model, altered) + with pytest.raises(ValueError, match='parent_reference_handoff_source_changed'): + registry.pose_source(task, 1) + + +@pytest.mark.parametrize('finger', ['pinky', 'ring', 'middle', 'index']) +def test_parent_model_ownership_applies_to_every_finger(capture, finger): + profile = capture[0] + registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references + task = next(t for t in profile.motion.tasks if t.key == finger+'_dip_side') + original = (object(), {'image_model_sha256':'a'*64}) + latest = (object(), {'image_model_selection_policy':SHARED_TAG_HANDOFF_POLICY, + 'shared_tag_pose_bridges':[{'joint':finger+'_pip', 'source_model_sha256':'a'*64, + 'source_reference':{'joint':finger+'_mcp_pitch'}}]}) + registry.models[(1, 'side', finger+'_mcp_pitch')] = original + registry.models[(1, 'side', finger+'_pip')] = latest + assert registry.pose_source(task, 1) is latest + + +def test_recovery_preserves_raw_epoch_and_restart_drops_live_grant(capture): + from linkerhand_calibration.runtime.shared_pose_handoff import SharedPoseHandoff + profile, _, _, data, reference = capture + cache = SharedPoseHandoff(profile) + for row in data['records']: cache.observe(row) + old = cache.rows(1, reference, 'side') + assert not cache.rows(2, reference, 'side') + cache.advance_recovery_epoch(1, 2) + assert cache.rows(2, reference, 'side') == old + assert all(r['session_epoch'] == 1 for r in old) + assert not SharedPoseHandoff(profile).rows(2, reference, 'side') + + +def test_dip_parent_check_uses_formal_capture_and_roundtrip(capture, replay): + """Replay real held pixels as a simulated later DIP visit, not a real DIP run.""" + import numpy as np + from dataclasses import asdict + from types import SimpleNamespace + from scipy.spatial.transform import Rotation + from linkerhand_calibration.runtime.capture import ObservationCapture, CaptureFrame + from linkerhand_calibration.runtime.reference_lock import ReferenceLock + from linkerhand_calibration.runtime.parent_reference_evidence import validate_parent_references + from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 + from linkerhand_calibration.core.geometry.tag_pose.cad_image_model import TransferredImageMotionModel + from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry + from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry + from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation + profile, _, request, data, reference = capture + registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references + source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') + restore_parent_models(registry, [source, replay[2]], 1) + task = next(t for t in profile.motion.tasks if t.key == 'pinky_dip_side') + root = replay[0].model.reference_frames[0].pose + rotation = Rotation.from_quat(reference.samples[0].parent_pose_common.quaternion_xyzw) + transform = np.eye(4) + transform[:3, :3] = rotation.as_matrix() + transform[:3, 3] = np.asarray(reference.samples[0].parent_pose_common.translation_xyz_m)-rotation.apply(root.translation_xyz_m) + host = ObservationCapture(profile, reference_lock=ReferenceLock({'side':3}), + extrinsics=SimpleNamespace(transform=lambda _:transform)) + registry.activate(task, 1, host) + target = profile.motion.joint_zero_references['pinky_dip'].command + motion = MotionCommand('joint_zero', target, 200., task_key=task.key, + zero_joints=('pinky_mcp_pitch',), reference_only=True, reference_reuse=True) + now = max(replay[0].model.source_stamps_ns)+1_000_000_000 + journal, samples = list(replay[3]), [] + for i, frame in enumerate(request.pose_bridges[0].frames): + rows, _ = host.consume(CaptureFrame('side', now+i*40_000_000, + np.asarray(frame.camera_matrix), {o.role:np.asarray(o.corners_xy) for o in frame.observations}, + reference.samples[0].feedback, target), motion) + journal.extend(rows) + samples.extend(r for r in rows if r['kind'] == 'joint_zero_sample') + assert len(samples) == 10 + check = registry.confirm(task, 1, 80, reference, samples) + assert check['schema_version'] == 2 and check['pose_model_joint'] == 'pinky_pip' + assert check['source_reference'] == reference.as_record() + journal.append(check) + hinges, check_id = registry.geometry(task, 1) + # A synthetic downstream model envelope exercises only the provenance + # boundary. Its DIP geometry is not presented as measured hardware data. + parent = hinges[0].geometry + axis = Rotation.from_quat(parent.reference_quaternion_xyzw).inv().apply(parent.axis_parent_xyz) + normal = np.cross(axis, [1.,0.,0.]); normal /= np.linalg.norm(normal) + child = ImageHingeGeometry(MotionRelation('pinky_dip', 'pinky_pip', 'pinky_dip'), + tuple(axis), (0.,0.,0.,1.), tuple(.03*normal-np.asarray(parent.mount_child_xyz_m)), (.015,0.,0.)) + later = now+2_000_000_000 + transferred = TransferredImageMotionModel((child,), replay[0].model.camera_matrix, + (('pinky_pip',.0165),('pinky_dip',.0165)), (later,later+1), (later,), (later+1,), + (('pinky_pip',0),('pinky_dip',0)), + constraints=(ParallelAxisGeometry('pinky_pip','pinky_dip',1,.03),), + source_urdf_sha256='d'*64, source_hinges=hinges) + evidence = _initializations(replay[3]) + evidence['synthetic_dip'] = dict(frozen_image_model=asdict(transferred), + image_model_sha256=image_model_sha256(asdict(transferred)), view='side', tag_role='pinky_dip', + session_epoch=1, motion_version=82, task_name=task.key, + parent_reference_sha256=check_id, source_image_stamps=[later,later+1]) + validate_parent_references(profile, journal, evidence) + damaged = deepcopy(check) + damaged['pose_model_joint'] = 'pinky_mcp_pitch' + journal[-1] = damaged + evidence['synthetic_dip']['parent_reference_sha256'] = source_frame_sha256(damaged) + with pytest.raises(ValueError, match='parent_reference_handoff_source_changed'): + validate_parent_references(profile, journal, evidence) + downgraded = deepcopy(check) + downgraded['schema_version'] = 1 + downgraded.pop('pose_model_joint'); downgraded.pop('pose_model_policy') + journal[-1] = downgraded + evidence['synthetic_dip']['parent_reference_sha256'] = source_frame_sha256(downgraded) + with pytest.raises(ValueError, match='parent_reference_handoff_policy_changed'): + validate_parent_references(profile, journal, evidence) diff --git a/src/linkerhand_calibration/test/test_staged_artifacts.py b/src/linkerhand_calibration/test/test_staged_artifacts.py new file mode 100644 index 0000000..09b49bf --- /dev/null +++ b/src/linkerhand_calibration/test/test_staged_artifacts.py @@ -0,0 +1,105 @@ +"""One complete formal O30 path, with shared artifacts for rejection tests.""" + +import json +import xml.etree.ElementTree as ET +from copy import deepcopy + +import pytest + +from linkerhand_calibration.core.domain.capture_plan import evidence_digest +from linkerhand_calibration.core.artifacts.storage import atomic_write_json +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json +from linkerhand_calibration.runtime.staged_training import resolve_staged_plan + +pytestmark = pytest.mark.integration + + +@pytest.fixture(scope='module') +def staged_artifacts(o30_capture, tmp_path_factory): + from staged_capture_fixture import staged_capture + directory = tmp_path_factory.mktemp('staged_formal') + profile, source, records, hashes, cameras, truth, frozen = staged_capture(o30_capture, directory) + session = directory/'session' + session.mkdir() + atomic_write_json(session/'frozen_training.json', frozen) + result = finalize_profile_session(profile=profile, source_urdf=source, records=records, + protected_inputs=hashes, serial_number='SYNTHETIC_FORMAL_O30', session_dir=session, + standard_loader=ET.parse, require_motion_evidence=True, camera_extrinsics_file=cameras, + frozen_training_sha256=evidence_digest(frozen), publish=False) + return profile, source, records, frozen, result, session, truth + + +def test_formal_staged_json_rebuilds_identical_urdf(staged_artifacts): + profile, source, records, frozen, (result, fit, artifact), session, truth = staged_artifacts + assert json.loads((session/'capture_validation.json').read_text())['passed'] + assert result['capture_plan']['policy'] == 'staged_2_to_3' + assert all(t['training_cycles'] == [0, 1] for t in result['capture_plan']['tasks'].values()) + assert all(fit.zero_method_by_joint[n] == 'assumed_source_cad_zero' for n in profile.zero.cad_zero_assumptions) + for name, expected in truth.items(): + assert fit.zero_offsets_rad[name] == pytest.approx(expected, abs=.004) + rebuilt = rebuild_urdf_from_json(artifact.staged_release.calibration_json, source, session/'rebuilt.urdf', + authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) + assert rebuilt.read_bytes() == artifact.path.read_bytes() + accepted = json.loads(artifact.staged_release.validation_json) + assert accepted['final_file_image_holdout_verified'] and accepted['urdf_rebuilt_from_json'] + assert not (session.parent/'latest_passed.json').exists() + + +def test_offline_decision_reproduces_original_freeze(staged_artifacts): + profile, source, records, *_ = staged_artifacts + restored = resolve_staged_plan(profile, records, source_urdf=source, verify_decisions=True, require_complete=True) + assert restored == profile + + +@pytest.mark.parametrize('fault', ['early_validation', 'post_freeze_training', 'decision_hash', 'late_reference']) +def test_rejects_invalid_phase_or_decision_without_another_fit(staged_artifacts, fault): + profile, _, records, *_ = staged_artifacts + changed = list(records) + row = next(r for r in records if r.get('kind') == 'joint_sample') + if fault == 'early_validation': + changed.insert(1, {**row, 'cycle': 3}) + elif fault == 'post_freeze_training': + changed.append(row) + elif fault == 'decision_hash': + index = next(i for i, r in enumerate(changed) if r.get('kind') == 'staged_training_decision') + changed[index] = {**changed[index], 'frozen_training_sha256': '0'*64} + else: + index = next(i for i, r in enumerate(changed) if str(r.get('verification', '')).startswith('staged_revisit')) + changed[index] = deepcopy(changed[index]) + for sample in changed[index]['current_reference']['samples']: + sample['image_stamp_ns'] += 10**16 + sample['sample_id'] = f"{sample['view']}:{sample['image_stamp_ns']}" + with pytest.raises(ValueError, match='staged_'): + resolve_staged_plan(profile, changed, require_complete=True) + + +def test_resume_keeps_complete_visits_and_preserves_frozen_prefix(staged_artifacts, tmp_path): + from linkerhand_calibration.runtime.engine import CalibrationEngine + from linkerhand_calibration.runtime.joint_resume import JointResume + from linkerhand_calibration.runtime.staged_resume import stage_visits, load_frozen_checkpoint + profile, source, records, frozen, *_ = staged_artifacts + last = next(i for i, r in enumerate(records) if r.get('kind') == 'joint_sample' and r.get('cycle') == 1 + and r.get('task_name') == profile.motion.tasks[2].key) + partial = JointResume(profile) + stage_visits(partial, CalibrationEngine(profile), records[:last+1]) + assert not any(key[0] == profile.motion.tasks[2].key and key[1] == 1 for key in partial.units) + assert {u.identity for u in CalibrationEngine(profile).scan_units() if u.cycle == 0} <= partial.units + frozen_end = next(i for i, r in enumerate(records) if r.get('kind') == 'staged_training_decision')+1 + resumed = JointResume(profile) + stage_visits(resumed, CalibrationEngine(profile), records[:frozen_end]) + assert {u.identity for u in CalibrationEngine(profile).scan_units() if u.cycle != 3} == resumed.units + atomic_write_json(tmp_path/'frozen_training.json', frozen) + assert load_frozen_checkpoint(profile, records[:frozen_end], tmp_path/'raw_samples.jsonl', source) == frozen + atomic_write_json(tmp_path/'frozen_training.json', {**frozen, 'solver_version': 'tampered'}) + with pytest.raises(ValueError, match='frozen_model_changed'): + load_frozen_checkpoint(profile, records[:frozen_end], tmp_path/'raw_samples.jsonl', source) + + +def test_cancel_before_finalization_never_writes_or_publishes(staged_artifacts, tmp_path): + profile, source, records, *_ = staged_artifacts + with pytest.raises(ValueError, match='operator_abort'): + finalize_profile_session(profile=profile, source_urdf=source, records=records, + protected_inputs={}, serial_number='SYNTHETIC_FORMAL_O30', session_dir=tmp_path, + cancelled=lambda: True) + assert not tuple(tmp_path.iterdir()) diff --git a/src/linkerhand_calibration/test/test_staged_runtime.py b/src/linkerhand_calibration/test/test_staged_runtime.py new file mode 100644 index 0000000..b33ab31 --- /dev/null +++ b/src/linkerhand_calibration/test/test_staged_runtime.py @@ -0,0 +1,99 @@ +"""Visit scheduling, bounded recovery and the persistent training service.""" + +import time + +import pytest + +from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training, evidence_digest +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.staged_training import dependency_tasks, staged_snapshot + + +def decision(profile, action, tasks=()): + row = dict(kind='staged_training_decision', decision=action, supplement_tasks=list(tasks), + input_plan_sha256=CapturePlan.from_profile(profile).sha256, failures=['persistent'] if action == 'fail' else []) + return {**row, 'decision_sha256': evidence_digest(row)} + + +@pytest.mark.parametrize('scope,final', [('local', 'freeze'), ('global', 'freeze'), ('local', 'fail')]) +def test_one_dependency_complete_supplement_then_freeze_or_fail(o30_capture, scope, final): + profile, _, records, _ = o30_capture + profile = configure_training(profile, STAGED) + execution = SessionExecution(profile) + session = execution.session + session._unit_index = max(i for i, u in enumerate(session._units) if u.cycle == 1) + session.phase = Phase.EVALUATE + tasks = dependency_tasks(profile, 'middle_pip' if scope == 'local' else 'palm_and_static_zero') + assert execution.evaluate(records, training_decision=decision(profile, 'supplement', tasks)).passed + assert session.current_unit.cycle == 2 + assert {u.task_key for u in session._units if u.cycle == 2} == set(tasks) + if scope == 'local': + assert len(tasks) < len(profile.motion.tasks) + session._unit_index = max(i for i, u in enumerate(session._units) if u.cycle == 2) + session.phase = Phase.EVALUATE + assert execution.evaluate(records, training_decision=decision(execution.profile, final)).passed + if final == 'freeze': + assert session.current_unit.cycle == 3 and session.phase == Phase.PREPARE + else: + assert session.phase == Phase.FAILED + + +def test_incremental_snapshot_equals_raw_evidence_and_invalidates_only_changed_task(o30_capture): + from linkerhand_calibration.runtime.capture_index import CaptureRecordIndex + profile, _, records, _ = o30_capture + profile = configure_training(profile, STAGED) + index = CaptureRecordIndex(retain_complete=False, retain_training_geometry=True) + index.extend(records) + first = staged_snapshot(profile, index) + assert evidence_digest(first) == evidence_digest(staged_snapshot(profile, records)) + task = profile.motion.tasks[0] + saved = index.training_task_snapshot(task.key, (0, 1)) + index.append({'kind': 'scan_unit_complete', 'task_name': profile.motion.tasks[-1].key, 'cycle': 1, 'passed': True}) + assert index.training_task_snapshot(task.key, (0, 1)) is saved + assert all(r['cycle'] in (0, 1) for r in first) + + +@pytest.mark.integration +def test_worker_reuses_one_process_and_stops_after_close(o30_capture): + from linkerhand_calibration.runtime.training import TrainingWorker + profile, source, _, _ = o30_capture + worker = TrainingWorker(timeout_seconds=10) + request = dict(profile=configure_training(profile, STAGED), source_urdf=source, rows=(), references={}) + try: + pid = None + for _ in range(2): + deadline, result = time.monotonic()+10, None + while result is None and time.monotonic() < deadline: + result = worker.poll(**request) + time.sleep(.005) + assert result['decision']['decision'] == 'supplement' + current = worker._process.pid + assert pid is None or pid == current + pid = current + finally: + worker.close() + with pytest.raises(RuntimeError, match='cancelled'): + worker.poll(**request) + + +def test_retry_allowance_survives_checkpoint_and_never_replaces_zero(o30_capture): + from linkerhand_calibration.runtime.motion_execution import MotionCommand + from linkerhand_calibration.runtime.zero_recovery import ZeroRecovery + from linkerhand_calibration.runtime.staged_resume import restore_recovery + profile = configure_training(o30_capture[0], STAGED) + execution, recovery = SessionExecution(profile), ZeroRecovery() + unit = execution.session._units[0] + name = profile.motion.tasks[0].joints[0] + motion = MotionCommand('joint_zero', profile.command.baseline_values, 200., + task_key=unit.task_key, zero_joints=(name,), reference_check=True) + assert recovery.take(motion, geometry_error='', reports={}, referenced_joints={name}) == 'retry_zero_images' + rows = [dict(kind='joint_zero_recovery', task_name=unit.task_key, joints=[name]), + dict(kind='scan_unit_complete', task_name=unit.task_key, cycle=unit.cycle, direction=unit.direction, + attempt=1, passed=False)] + restored = ZeroRecovery() + restore_recovery(execution.session, restored, rows) + assert restored.take(motion, geometry_error='', reports={}, referenced_joints={name}) is None + assert execution.session._retry_by_unit[unit.identity] == 1 + with pytest.raises(ValueError, match='recovery_exhausted'): + restore_recovery(execution.session, restored, [{**rows[-1], 'attempt': 2}]) diff --git a/src/linkerhand_calibration/test/test_staged_training.py b/src/linkerhand_calibration/test/test_staged_training.py new file mode 100644 index 0000000..70478d2 --- /dev/null +++ b/src/linkerhand_calibration/test/test_staged_training.py @@ -0,0 +1,162 @@ +"""The staged policy shares evidence contracts with the fixed collector.""" + +import json +import gzip +from dataclasses import replace +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training, select_task_training +from linkerhand_calibration.core.domain.reference import read_joint_zero_references +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase +from linkerhand_calibration.runtime.staged_training import ( + assess_staged_training, staged_snapshot, read_frozen_training, dependency_tasks, +) + + +@pytest.fixture(scope='module') +def staged_fit(o30_capture): + profile, source, records, truth = o30_capture + profile = configure_training(profile, STAGED) + references = read_joint_zero_references(profile, records) + rows = staged_snapshot(profile, records) + result = assess_staged_training(profile=profile, source_urdf=source, rows=rows, references=references) + assert result['decision']['decision'] == 'freeze', result['decision'] + return profile, source, records, references, rows, result + + +def test_all_first_visits_precede_second_and_validation_keeps_original_motion(): + fixed = load_bundled_hand_profile('o30_right_18') + profile = configure_training(fixed, STAGED) + units = CalibrationEngine(profile).scan_units() + assert CapturePlan.from_profile(profile).as_dict()['version'] == 'capture_plan_v2' + assert [u.cycle for u in units] == sorted(u.cycle for u in units) + assert units == tuple(sorted((u for u in CalibrationEngine(fixed).scan_units() if u.cycle != 2), key=lambda u: u.cycle)) + selected = select_task_training(profile, 'fingers_mcp_roll_front', (0, 1, 2)) + assert len([u for u in CalibrationEngine(selected).scan_units() if u.cycle == 2]) == 4 + assert fixed.quality.training_policy == 'fixed' + + +def test_revisit_reuses_model_but_checks_zero_via_existing_path(): + profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) + execution = SessionExecution(profile) + task = profile.motion.tasks[0] + execution.zero_references.update({name: None for name in task.joints}) + session = execution.session + session._unit_index = next(i for i, u in enumerate(session._units) if u.task_key == task.key and u.cycle == 1) + session.phase = CalibrationPhase.PREPARE + effects = execution._make_effects(profile.command.baseline_values) + checks = [e for e in effects if e.phase == 'joint_zero' and not e.reference_only] + assert checks and all(e.reference_check for e in checks) + assert all(e.reference_check for e in effects if e.phase == 'zero_approach' and not e.reference_only) + + +@pytest.fixture(scope='module') +def recorded_revisit(): + from linkerhand_calibration.core.domain.reference import JointZeroReference + data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_revisit_feedback.json.gz').read_bytes())) + return (load_bundled_hand_profile('o30_right_18'), + JointZeroReference.from_record(data['previous']), JointZeroReference.from_record(data['current'])) + + +def test_revisit_real_feedback_repeatability_and_legacy_contract(recorded_revisit): + from linkerhand_calibration.runtime.revisit import revisit_record, verify_revisit + profile, previous, current = recorded_revisit + with pytest.raises(ValueError, match='held_feedback_changed'): + verify_revisit(profile, previous, current) # v1 retains the old limit. + row = revisit_record(profile, previous, current, 0) + assert row['feedback_limits'] == dict(unit='u8', within_hold_range=2., between_visit_difference=10.) + assert row['preserved_reference'] == previous.as_record() + verify_revisit(profile, previous, current, verification=row['verification'], feedback_limits=row['feedback_limits']) + with pytest.raises(ValueError, match='feedback_policy_changed'): + verify_revisit(profile, previous, current, verification=row['verification'], + feedback_limits={**row['feedback_limits'], 'between_visit_difference': 11.}) + with pytest.raises(ValueError, match='policy_unknown'): + verify_revisit(profile, previous, current, verification='staged_revisit_v999') + + +@pytest.mark.parametrize('fault', ['at_limit', 'over_limit', 'unstable', 'pose', 'installation']) +def test_revisit_repeatability_does_not_replace_stability_or_geometry(recorded_revisit, fault): + from linkerhand_calibration.runtime.revisit import revisit_record + profile, previous, current = recorded_revisit + samples = [] + for i, sample in enumerate(current.samples): + feedback = list(sample.feedback) + if fault in {'at_limit', 'over_limit'}: + feedback[5] = 48. - (10. if fault == 'at_limit' else 10.01) + elif fault == 'unstable': + feedback[5] = 41. + (3. if i == 0 else 0.) + sample = replace(sample, feedback=tuple(feedback)) + if fault == 'pose': + sample = replace(sample, child_pose_common=replace(sample.child_pose_common, + translation_xyz_m=(1., 1., 1.))) + samples.append(sample) + current = replace(current, samples=tuple(samples)) + if fault == 'installation': + current = replace(current, branch_references=()) + if fault == 'at_limit': + assert revisit_record(profile, previous, current, 0)['preserved_reference'] == previous.as_record() + else: + with pytest.raises(ValueError, match='revisit_reference_changed'): + revisit_record(profile, previous, current, 0) + + +def test_revisit_new_feedback_policy_is_scoped_to_o30(): + from linkerhand_calibration.runtime.revisit import revisit_feedback_limits + for name in ('o6_right_8', 'g20_right_19'): + limits = revisit_feedback_limits(load_bundled_hand_profile(name)) + assert limits['between_visit_difference'] == 2*limits['within_hold_range'] + + +def test_dependency_closure_keeps_complete_observation_tasks(): + profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) + tasks = dependency_tasks(profile, 'middle_pip') + assert 'middle_dip_side' in tasks + assert 'middle_pip_side' in tasks + assert set(dependency_tasks(profile, 'shared_palm_geometry')) == {t.key for t in profile.motion.tasks} + + +@pytest.mark.integration +def test_two_training_rounds_freeze_once_and_final_validation_does_not_fit(staged_fit, monkeypatch): + from linkerhand_calibration.runtime.acquisition import accepted_joint_records + from linkerhand_calibration.core.fitting.session import fit_profile_calibration + profile, source, records, references, rows, result = staged_fit + payload = json.loads(json.dumps(result['frozen_training'])) + frozen = read_frozen_training(profile, rows, references, source, payload) + def forbidden(*args, **kwargs): + raise AssertionError('frozen training must not run another optimizer') + monkeypatch.setattr('linkerhand_calibration.core.fitting.command_motion.fit_command_training', forbidden) + monkeypatch.setattr('linkerhand_calibration.core.fitting.spatial_solver.solve.solve_selected', forbidden) + kept = [r for r in records if r.get('cycle') != 2 or r.get('kind') == 'joint_zero_reference'] + fit = fit_profile_calibration(profile, source, accepted_joint_records(profile, kept), + zero_references=references, steady_records=accepted_joint_records(profile, kept, sample_phase='steady'), + frozen_training=frozen) + assert len(fit.output_mappings) == 20 + with pytest.raises(ValueError, match='frozen_hand_training_inputs_changed'): + read_frozen_training(profile, rows[:-1], references, source, payload) + + +@pytest.mark.integration +def test_holdout_is_never_a_training_input(staged_fit): + profile, source, records, references, rows, result = staged_fit + poisoned = [dict(r, relative_quaternion_xyzw=[float('nan')]*4) if r.get('cycle') == 3 else r for r in records] + assert staged_snapshot(profile, poisoned) == rows + with pytest.raises(ValueError, match='received_holdout'): + assess_staged_training(profile=profile, source_urdf=source, rows=({**rows[0], 'cycle': 3},), references=references) + + +@pytest.mark.integration +def test_mixed_rounds_count_only_complete_real_dependencies(o30_capture): + profile, source, records, _ = o30_capture + profile = configure_training(profile, STAGED) + for key in dependency_tasks(profile, 'middle_pip'): + profile = select_task_training(profile, key, (0, 1, 2)) + result = assess_staged_training(profile=profile, source_urdf=source, rows=staged_snapshot(profile, records), + references=read_joint_zero_references(profile, records)) + assert result['decision']['decision'] == 'freeze', result['decision'] + counts = result['decision']['metrics']['spatial']['independent_cycles_by_joint'] + assert counts['middle_pip'] == 3 and counts['pinky_pip'] == 2 diff --git a/src/linkerhand_calibration/test/test_zero_recovery.py b/src/linkerhand_calibration/test/test_zero_recovery.py index e1e870b..2af489f 100644 --- a/src/linkerhand_calibration/test/test_zero_recovery.py +++ b/src/linkerhand_calibration/test/test_zero_recovery.py @@ -52,7 +52,8 @@ def test_transient_preparation_retries_locally_and_persistent_failure_stops(tmp_ run_until(host, clock, lambda: host.state == "PAUSED" or bool(host.execution.zero_references)) assert len(requests) == 2 assert max(frame.stamp_ns for frame in requests[0].frames) < min(frame.stamp_ns for frame in requests[1].frames) - assert len([r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"]) == 1 + recoveries = [r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"] + assert len(recoveries) == 1 and recoveries[0]['failure_category'] == 'missing_samples' if persistent: assert host.state == "PAUSED" and not host.execution.zero_references else: