O30标定逻辑修改
This commit is contained in:
@@ -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
|
||||
@@ -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` 同时写入初始化
|
||||
报告和按角色哈希绑定的证据。辅助运动、独立准备、断点与最终验收的重新拟合均读取
|
||||
这项策略；无字段的旧日志继续使用原预算。删除策略、改写策略或使训练/留出帧数超过
|
||||
对应预算会拒绝回读，不能让在线与离线分别选择不同的子集。
|
||||
|
||||
@@ -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 始终为独立验证。
|
||||
|
||||
@@ -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%
|
||||
才考虑切换默认。大日志存储/全量解码重构未与本次采集规则一起进行。
|
||||
@@ -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。
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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()})
|
||||
@@ -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),
|
||||
|
||||
+11
-18
@@ -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
|
||||
|
||||
+42
-9
@@ -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
|
||||
|
||||
+42
-3
@@ -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):
|
||||
|
||||
+70
@@ -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
|
||||
+10
@@ -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:
|
||||
|
||||
+15
-6
@@ -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)
|
||||
|
||||
+3
-4
@@ -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)))
|
||||
|
||||
+3
-8
@@ -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)
|
||||
|
||||
+148
@@ -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)
|
||||
+42
-4
@@ -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"
|
||||
|
||||
+54
-9
@@ -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)
|
||||
|
||||
+3
-4
@@ -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.
|
||||
|
||||
+213
@@ -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)
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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')
|
||||
@@ -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)
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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,
|
||||
|
||||
+16
-4
@@ -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,
|
||||
|
||||
@@ -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,
|
||||
|
||||
+160
-11
@@ -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."""
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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())
|
||||
|
||||
@@ -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"]))
|
||||
|
||||
@@ -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 (), ()
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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')
|
||||
@@ -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
|
||||
|
||||
|
||||
+21
-2
@@ -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))
|
||||
|
||||
@@ -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)
|
||||
@@ -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
|
||||
+123
@@ -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')
|
||||
@@ -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
|
||||
+126
@@ -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')
|
||||
@@ -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
|
||||
@@ -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": "开始请求未收到确认，不能判断任务是否已启动；运行栈将退出。",
|
||||
|
||||
@@ -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()}
|
||||
@@ -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,
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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":
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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}:
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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]
|
||||
@@ -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 安装验证姿态",
|
||||
|
||||
@@ -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")):
|
||||
|
||||
@@ -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"}
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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.
|
||||
|
||||
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -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
|
||||
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
@@ -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])
|
||||
@@ -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)
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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',)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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())
|
||||
@@ -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}])
|
||||
@@ -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
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user