离线拟合
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.8138585956461776
|
||||||
|
- -0.008439712508702443
|
||||||
|
- 1.0397597678627144
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.017450615663592975
|
||||||
|
- 0.6859174861464342
|
||||||
|
- 0.01494743127074939
|
||||||
|
- 0.7273164734212502
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.03984872898745615
|
||||||
|
- -0.5795215841986667
|
||||||
|
- 1.0606733822933996
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.7006937955854443
|
||||||
|
- -0.05305859990878122
|
||||||
|
- -0.03489268952144818
|
||||||
|
- 0.7106303469608818
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 0.8784854263662394
|
||||||
|
maximum_rotation_repeatability_deg: 0.03530519844936986
|
||||||
|
maximum_translation_repeatability_m: 0.0005675535417705807
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -0,0 +1,111 @@
|
|||||||
|
# O30 自动生成链路修复与实采验证
|
||||||
|
|
||||||
|
2026-09-23。本次实际修改生产代码并只读回放两批实采数据，未驱动硬件。
|
||||||
|
采集与离线求解已解耦，行程端点约束及 JSON→URDF 导出链已修复；当前生成的是
|
||||||
|
**未通过实测精度认证的候选**，不能作为正式正确版本发布。
|
||||||
|
修复前分析见 [O30_CURRENT_FLOW_REVIEW.md](O30_CURRENT_FLOW_REVIEW.md)。
|
||||||
|
剩余误差已按机位、标签和指令进一步定位，见
|
||||||
|
[O30_REMAINING_ISSUES.md](O30_REMAINING_ISSUES.md)。
|
||||||
|
|
||||||
|
## 1. 收敛后的流程
|
||||||
|
|
||||||
|
1. 启动前保存本次有效 Product、Profile、CAD URDF/mesh、内外参及标签配置。
|
||||||
|
默认配置同步已确认的四指 SDK 方向、中节标签绑定和新外参；历史会话仍按自身
|
||||||
|
快照及外参回放，不能将新外参套在旧图像上。
|
||||||
|
2. 保留既有路线、速度、2＋1 分区、108 方向、一次局部补采及停滞重试保护。
|
||||||
|
采集完成先归位封存，关闭 SDK、相机、监控及 ROS 控制上下文，再写采集结果。
|
||||||
|
完整返回 0、缺项返回 3；均不进入拟合，也不因离线失败重采。
|
||||||
|
3. 使用会话 `capture_product.yaml` 和 `raw_samples.jsonl` 单独离线拟合。
|
||||||
|
原始文件与配置、mesh 均受清单哈希保护。验证轮 3 不参与参数优化或候选选择。
|
||||||
|
4. 先校验草稿 JSON，再读回 JSON 生成 URDF；共用一个导出函数，检查实际输出文件
|
||||||
|
FK 与标准加载。独立报告 JSON 双向曲线和纯 URDF 上下限线性驱动的精度。
|
||||||
|
5. 草稿允许保留失败证据用于诊断，精度失败不会更新 `latest_passed`。
|
||||||
|
|
||||||
|
普通单批离线入口及参数见 [README.md](README.md#低精度离线修正草稿o30)。
|
||||||
|
本次跨批验证脚本保存在仓库根目录下
|
||||||
|
`calibration_output/software_fix_20260923_auto_urdf_v2/replay.py`。
|
||||||
|
它调用包内拟合、范围证据、导出和验收函数，不另写一套 URDF 生成逻辑。
|
||||||
|
脚本对输入和实现哈希有缓存约束；代码变化后不能直接复用旧参数冒充重新验证。
|
||||||
|
|
||||||
|
## 2. 行程问题的修复
|
||||||
|
|
||||||
|
换向处通常只保存到达端点的方向标签；离开端点的另一个方向没有独立端点观测。
|
||||||
|
旧模型仍给两个方向分别分配端点自由参数，缺少直接约束的端点可能保留初值，
|
||||||
|
或由邻近指令外推，并进入最终 URDF 极值。这会使文件可加载而行程依据不足。
|
||||||
|
|
||||||
|
新模型对这类端点施加明确的换向连续性约束：有观测的到达端点同时决定未观测的
|
||||||
|
离开端点。如果两方向均有端点观测，则保留独立参数；若连来源端点都缺失，拒绝
|
||||||
|
导出该行程。该假设不等同于两次独立测量，使用
|
||||||
|
`observed_turnaround_continuity_v1` 明确记录。
|
||||||
|
|
||||||
|
优化只移除声明为从属的参数，其他秩亏仍拒绝；图像 Jacobian、角度导数和范围
|
||||||
|
不确定度均沿同一依赖传播，避免只修改导出数字。证据记录训练样本、轮次和方向，
|
||||||
|
JSON 回读检查端点连续性及来源。测试确认任意扰动从属参数不改变残差或行程。
|
||||||
|
|
||||||
|
普通 URDF 的上下限只表示可动范围，不能编码 SDK 指令对应的非线性、双向曲线。
|
||||||
|
因此增加 `draft_urdf_linear_holdout.json`，专门检查
|
||||||
|
`q = lower + (upper - lower) × u / 255`（按 SDK 方向处理）的实际误差。
|
||||||
|
曲线验证通过不能被当成该线性驱动方式通过；零位只应用一次，非零指令基准也不
|
||||||
|
强行当作线性驱动的零角度。
|
||||||
|
|
||||||
|
## 3. 两批真实数据的结果
|
||||||
|
|
||||||
|
| 数据 | 原始方向完成量 | 训练/验证帧数 | 用途 |
|
||||||
|
| --- | ---: | ---: | --- |
|
||||||
|
| 旧外参 `20260923_142201` | 107/108 | 4769 / 2681 | 重新拟合全手参考 |
|
||||||
|
| 新外参 `20260923_183138` | 24/24 | 922 / 516 | 固定旧四指参考后更新拇指 |
|
||||||
|
|
||||||
|
仅取封存通过的最后尝试及有效稳态帧，不拼接失败尝试。旧数据缺第 1 训练轮
|
||||||
|
`fingers_mcp_roll_front / increasing / linked_increasing`，不能由验证轮补齐。
|
||||||
|
旧四指侧摆保留此前确认的竖直定义基准，四项在 JSON 标为用户定义；五个末端
|
||||||
|
零位保留 CAD。不能将这九项说成此次新独立测量的零位。
|
||||||
|
跨批旧参考仍是条件参考，其系统误差没有完整传播，不能据此正式认证新拇指。
|
||||||
|
|
||||||
|
两个训练阶段均在原有预算内收敛，先冻结参数再验证。原始数据哈希保持不变。
|
||||||
|
最终候选位于：
|
||||||
|
|
||||||
|
- `calibration_output/software_fix_20260923_auto_urdf_v2/artifacts/o30_right_draft.urdf`
|
||||||
|
- `calibration_output/software_fix_20260923_auto_urdf_v2/artifacts/draft_calibration.json`
|
||||||
|
|
||||||
|
同目录 mesh 自包含；JSON 重建 URDF 字节一致，SHA256 为
|
||||||
|
`ecb36313311292cb1ebe37d8069ba18234ac1dd6e5ccda39902d10e74ed862ac`。
|
||||||
|
最终文件 FK 最大差 `6.66e-16`，ROS 标准加载及 MuJoCo 20 关节加载/前向计算通过。
|
||||||
|
根目录另有早期诊断文件，本次最终路径以 `result.json` 的 `artifacts/` 路径为准。
|
||||||
|
|
||||||
|
以下角度误差来自冻结模型的独立验证轮，单位为度；测量本身未通过全部图像及
|
||||||
|
不确定度门限，所以它们用于定位问题，不是物理真值精度证书。
|
||||||
|
|
||||||
|
| 拇指关节 | 生成上限 | 曲线最大角度误差 | 纯 URDF 线性最大误差 |
|
||||||
|
| --- | ---: | ---: | ---: |
|
||||||
|
| CMC roll | 52.916 | 0.837 | 0.910 |
|
||||||
|
| CMC yaw | 110.843 | 1.549 | 3.483 |
|
||||||
|
| MCP | 105.135 | 0.748 | 1.325 |
|
||||||
|
| IP | 85.484 | 1.066 | 2.625 |
|
||||||
|
|
||||||
|
当前不能发布的具体原因：
|
||||||
|
|
||||||
|
- 新拇指曲线角度子项通过，但独立角度测量最大重投影误差仍为 7.194 px，最大
|
||||||
|
3σ 不确定度约 1.733°，未通过原门限，不能只报告角度子项通过。
|
||||||
|
- 纯 URDF 线性驱动下 yaw 最大误差 3.483°；IP 的平均/95 分位误差为
|
||||||
|
1.557°/2.571°，这两个关节的角度子项失败。
|
||||||
|
- 旧四指曲线测量最大重投影误差 16.468 px、最大 3σ 约 8.713°；线性驱动验证
|
||||||
|
的独立角度求解未收敛，保留 `raw_joint_holdout_angle_measurement_failed`。
|
||||||
|
- 旧采集不完整，且跨批条件参考尚不满足正式认证。
|
||||||
|
|
||||||
|
下一步应先用已封存图像定位失败机位/标签的重投影残差和参考不确定度，验证
|
||||||
|
标签安装、相机几何及观测条件。只有数据证明需要时，再决定补采范围或更改模型。
|
||||||
|
需要保持 SDK 字节轨迹时，运行端应使用经验证的 JSON 曲线得到弧度并驱动 URDF；
|
||||||
|
若产品要求单靠上下限线性驱动，则必须以该独立报告为准，不能只扩大上限消除表象。
|
||||||
|
|
||||||
|
## 4. 软件回归与证据
|
||||||
|
|
||||||
|
采集/局部/重装组 43 项、离线导出/端点组 43 项、更新审计组 3 项，以及真实
|
||||||
|
coordinator 的完整 108 方向虚拟时钟集成 1 项通过。最新配置及 mesh 封存修改后，
|
||||||
|
对应 4 项边界检查再次通过；它们与前述用例重叠，不重复累计。
|
||||||
|
完整调度用例耗时约 97 秒，其余组分别约 17、34、0.3 和 2.4 秒。
|
||||||
|
软件调度回归不代表本轮实机连续采集通过。
|
||||||
|
|
||||||
|
产物目录的 `verification.json`、`mujoco_acceptance.json`、`curve_holdout.json`、
|
||||||
|
`linear_holdout.json`、`training_frozen.json`、`result.json` 及 `test_evidence/`
|
||||||
|
分别保存来源核验、加载、精度、冻结输入和软件测试记录。
|
||||||
|
本次未放宽精度/可观测性门限，未替换仿真默认模型，未发布正式版本。
|
||||||
@@ -0,0 +1,180 @@
|
|||||||
|
# O30 当前采集与自动生成链路复核
|
||||||
|
|
||||||
|
本文为修复前的只读复核记录；后续实际改动、两批数据重拟合及验收结论见
|
||||||
|
[O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
2026-09-23。目标是完全由程序从采集数据生成正确、可复现的 JSON 和修正 URDF。
|
||||||
|
本次评估程序生成版，不用人工调整版作为拟合结果或精度依据。
|
||||||
|
|
||||||
|
结论：轻量采集、离线求解的方向合理，但目前还不能把“采完”或“草稿能加载”视为
|
||||||
|
“自动生成正确 URDF”。优先收敛采集边界和配置，再修复行程证据约束，最后用冻结
|
||||||
|
数据验证实际驱动方式。当前没有证据支持靠增大拇指上限或增加优化迭代解决全部偏差。
|
||||||
|
|
||||||
|
本轮只读复核代码和已有真实数据，运行针对性软件测试；未运动硬件、未重新拟合、
|
||||||
|
未修改生产代码/配置或替换模型。已有工作区改动和被删除的历史测试均保留。
|
||||||
|
|
||||||
|
## 1. 当前流程与合理之处
|
||||||
|
|
||||||
|
实际入口：`runtime/runner.py`；默认 `raw_joint_2_plus_1` / `raw_joint_images_v2`。
|
||||||
|
|
||||||
|
1. 启动检查、固定参考锁定、归基准；17 个任务覆盖 20 个关节。
|
||||||
|
2. 每任务训练轮 0/1、独立验证轮 3，共 108 个方向单元。
|
||||||
|
3. 移动标签在线只记录角点；保持反馈、同步、固定参考及硬件保护。质量不足的方向
|
||||||
|
立即补一次，不拼接失败尝试；恢复保留预算。硬件停滞重试与图像质量补采分别处理。
|
||||||
|
4. 归位、封存原始数据，状态为 `CAPTURE_COMPLETE`，随后关闭 SDK/相机/监控。
|
||||||
|
5. **启动器实际上还会自动调用草稿拟合**；显式 `--offline-raw` 则可单独离线运行。
|
||||||
|
|
||||||
|
值得保留：在线不依赖移动标签几何消歧；端点按已发送指令核验，不要求反馈恰好
|
||||||
|
达到 0/255；离线只取同步、有效稳态帧；验证轮不用于训练；原始证据封存；正式
|
||||||
|
发布保持来源、候选不确定度、独立验证及 JSON→URDF 回读门禁。
|
||||||
|
|
||||||
|
拟合固定 CAD 拓扑、连杆和关节轴，联合估计掌坐标、标签安装、关节零位和双向
|
||||||
|
分段指令曲线。15 个零位可估，五个末端零位沿用 CAD 假设；显式竖直侧摆草稿
|
||||||
|
另把四个侧摆零位设为用户定义基准，因此该草稿实际只估 11 个零位。
|
||||||
|
这些假设必须继续写进产物，不能改称全部零位都由视觉独立测得。
|
||||||
|
|
||||||
|
## 2. 优先处理的问题
|
||||||
|
|
||||||
|
### P0：采集完成仍绑定草稿求解结果
|
||||||
|
|
||||||
|
`runtime/runner.py:271` 在关闭控制栈后调用 `finish_captured_draft()`；
|
||||||
|
`runtime/artifacts/raw_joint_draft.py:352` 只有“数据完整且完整目标草稿生成”才返回 0。
|
||||||
|
因此即使采集完整，离线不收敛也会使整个命令返回 3。关闭控制栈的顺序是正确的，
|
||||||
|
但采集任务的耗时和成功语义仍受拟合影响，README 的“后续单独离线”也不完全准确。
|
||||||
|
|
||||||
|
建议默认入口结束于归位、封存和采集报告，退出码只描述采集；拟合由显式离线命令
|
||||||
|
执行。若保留一键流程，作为明确选项组合两个阶段，分别记录退出状态；拟合失败
|
||||||
|
不改变采集结论，更不据此安排重采。不要另建采集执行器。
|
||||||
|
|
||||||
|
### P0：默认配置与有效离线配置没有收敛
|
||||||
|
|
||||||
|
对照当前默认 Profile 与生成程序草稿使用的
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_profile.yaml`：
|
||||||
|
|
||||||
|
| 项目 | 默认 Profile | 草稿拟合 Profile |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| 四指侧摆通道 2–5 的 SDK→URDF 方向 | −1 | +1 |
|
||||||
|
| 正面 ID12–15 安装连杆 | `*_metacarpals` | `*_middle` |
|
||||||
|
| 默认产品外参 | 20260922_101452 | 新拇指批次使用 20260923_181333 |
|
||||||
|
|
||||||
|
ID17 在两者中均已绑定 `thumb_proximal`。这些差异不是哈希损坏：默认配置自己的
|
||||||
|
`--validate-only` 可以通过，但这只证明内部结构和文件身份一致，不证明与现场安装一致。
|
||||||
|
原始角点仍有价值，不能因为默认建模配置错误就全部废弃数据。
|
||||||
|
|
||||||
|
建议把已确认的模型方向/标签连杆修订纳入版本化配置；每次采集保存完整配置包，
|
||||||
|
包括 Product、Profile、源 URDF、内外参与标签尺寸。旧数据按自己的快照回放；
|
||||||
|
模型修订另存，保持原采集身份。外参按实际安装选择，不能无条件把“最新文件”
|
||||||
|
应用到所有历史数据。新旧相机批次应各用各的外参。
|
||||||
|
|
||||||
|
### P1：未观测的方向端点可以改变“实测行程”
|
||||||
|
|
||||||
|
这是本次用当前代码重新复现的确定问题，不是照片推测。
|
||||||
|
读取新拇指真实训练帧及已保存参数，核验原始数据 SHA256 与封存清单，不重新求解：
|
||||||
|
|
||||||
|
| 关节 | 程序 URDF 上限 ° | 超出已观测正程端点 ° | 未观测回程参数加 5° 后上限变化 |
|
||||||
|
| --- | ---: | ---: | ---: |
|
||||||
|
| CMC roll | 52.9662 | 0.0520 | +5° |
|
||||||
|
| CMC yaw | 110.9948 | 0.1495 | +5° |
|
||||||
|
| MCP | 106.0734 | 0.9447 | +5° |
|
||||||
|
| IP | 85.4810 | 0.0018 | +5° |
|
||||||
|
|
||||||
|
四项的训练 Jacobian 对应列范数都是 0；修改后训练残差逐元素完全不变。
|
||||||
|
255 端点有正程方向历史的图像，没有独立 decreasing/255 图像。回程起点沿用
|
||||||
|
上一实际推进方向是正确记录，不能把它伪标成独立回程观测。
|
||||||
|
|
||||||
|
原因链：`UrdfCommandImages` 为每方向分配完整曲线 → `JointMapping.bounds_rad`
|
||||||
|
取两条完整曲线包络 → `prepare_raw_motion_ranges()` 将它直接写入范围证据。
|
||||||
|
当前范围证据检查了基准、轮次及图像身份，没有约束每个极值的方向有效域。
|
||||||
|
增量更新工具虽然冻结无观测参数，却仍保留其初值，并未阻止初值进入导出范围。
|
||||||
|
|
||||||
|
本批模型调用全参数协方差检查会报 `urdf_images_geometry_rank_deficient`；上述
|
||||||
|
零列已足以造成秩亏。该检查包含固定旧四指的参数，不能当成一次完整正式拟合
|
||||||
|
运行结果，但说明“方向单元采全”不保证全部自由参数可辨识。
|
||||||
|
|
||||||
|
建议统一处理模型、曲线和导出的有效域：每方向节点记录训练支持、可辨识性和
|
||||||
|
来源；未观测自由参数不进入实测范围。若使用共享端点/连续性约束，必须显式
|
||||||
|
标为模型假设并检查数据是否支持。JSON 读取、范围证据、URDF 导出及协方差应
|
||||||
|
使用同一份契约，不能只在最后裁剪 limit，也不能靠伪逆把未知量变成零不确定度。
|
||||||
|
|
||||||
|
必要回归：扰动无观测参数不得改变认证范围；训练/验证不串用；方向切换端点身份
|
||||||
|
正确；缺证据时明确报告范围不足；JSON 回读重建保持同一有效域。
|
||||||
|
|
||||||
|
### P1：JSON 曲线精度不等于纯 URDF 线性驱动精度
|
||||||
|
|
||||||
|
审查程序生成文件：`offline_combined_new_thumb_draft/o30_right_draft.urdf`，
|
||||||
|
SHA256：`9d6e71276ffcc2108cf0776cdee08a23803de9f048b1a42b7ca76e98b40e48a2`。
|
||||||
|
复核保存的独立第三轮报告，最大角度误差如下；本轮没有用第三轮重新拟合：
|
||||||
|
|
||||||
|
| 关节 | JSON 双向曲线 ° | 纯 URDF 线性驱动 ° |
|
||||||
|
| --- | ---: | ---: |
|
||||||
|
| roll | 0.837 | 0.919 |
|
||||||
|
| yaw | 1.538 | 3.516 |
|
||||||
|
| MCP | 0.749 | 2.285 |
|
||||||
|
| IP | 1.070 | 2.621 |
|
||||||
|
|
||||||
|
曲线模式的四关节角度子项通过，但整体像素/不确定度仍失败；两个模式的整体
|
||||||
|
报告均为 `passed=false`。这些角度是条件图像模型的估计，不是独立编码器真值。
|
||||||
|
|
||||||
|
当前模型的修正关系是 `T_origin_corrected = T_origin_CAD · R_axis(delta)`，
|
||||||
|
JSON 给出相对修正零位的 `q_output=f(command, direction)`。文件回读 FK 一致
|
||||||
|
证明写入关系一致，不证明 delta 或 f 是真实物理值。
|
||||||
|
|
||||||
|
当前纯 URDF 驱动使用 `q=lower+(upper-lower)*u/255`。上下限能表达角度范围，
|
||||||
|
不能同时保存中间点非线性和方向差异。此前冻结同一模型的训练诊断中，yaw 在
|
||||||
|
SDK80 处线性值比正程曲线大约 2.90°，在 SDK181 处大约 1.32°；不是全区间都
|
||||||
|
“行程偏小”。当前 yaw 的未观测端点影响约 0.15°，也不足以解释全部偏差。
|
||||||
|
|
||||||
|
目标应明确为程序同时生成可信几何 URDF 和指令映射。如果使用纯 URDF 线性
|
||||||
|
驱动，就必须用该驱动方式独立验收零位、两端、中段及组合姿态，并如实报告无法
|
||||||
|
用一条直线达到的精度；不能把扩大物理行程作为非线性补偿。
|
||||||
|
|
||||||
|
### P1：实际重复性与组合姿态仍未达到自动认证条件
|
||||||
|
|
||||||
|
已有现场记录中，同基准指令返回后，正面固定 ID0 角点 RMS 位移约 0.092 px，
|
||||||
|
拇指 ID1/ID2 分别约 7.39/16.23 px，反馈接近。证据说明存在需要解释的重复性
|
||||||
|
差异，不能仅凭一次对照唯一归因于硬件回差、标签松动或零位错误。
|
||||||
|
当前双向曲线都强制基准值为 0，单个固定零位不能自动解释不一致的基准观测。
|
||||||
|
|
||||||
|
建议先用已有各轮基准帧做相同路径/方向/其他通道状态下的重复性统计，单独报告
|
||||||
|
跨路径差异。保持既有原始帧，不为降低残差删掉不利观测，也不立即增加在线几何
|
||||||
|
暂停。若固定条件下仍无法重复，增加优化迭代不能获得可信的确定性映射。
|
||||||
|
|
||||||
|
新拇指与旧四指组合依赖冻结的旧四指及标签安装参数。它是条件草稿；旧参考误差
|
||||||
|
未完整传播，旧整手仍有 1/108 方向缺项。新拇指 24/24 不能补齐这些认证缺口。
|
||||||
|
组合生成还依赖输出目录中的专用脚本，没有形成统一的跨批次正式入口。
|
||||||
|
|
||||||
|
## 3. 建议实施顺序及验收边界
|
||||||
|
|
||||||
|
1. **先稳定采集交付**：收敛配置与快照，默认只采集并封存；保留原路线、速度、
|
||||||
|
2+1 分区、一次补采和硬件保护。采集报告明确已完成/缺项/中断、是否安全归位。
|
||||||
|
2. **用已有数据修离线契约**：先解决方向有效域与未观测参数，再用相同冻结输入
|
||||||
|
比较旧/新结果。统一共享模型和导出模块，避免另建拇指拟合器或手调常量。
|
||||||
|
3. **形成单一可复现离线入口**：训练模型选择 → 冻结 → 独立轮验证 → JSON 落盘 →
|
||||||
|
重建 URDF → 实际驱动方式的文件/FK/角度验收。跨批次模型显式携带外参及参考
|
||||||
|
不确定度。失败另存报告，不覆盖已发布模型、不自动重采。
|
||||||
|
4. **最后评估实机精度**：先区分基准、端点、中间映射、组合几何的误差，再决定
|
||||||
|
是否缺少必要观测。只有数据能支持轴线/安装修正时才扩展模型，不能靠任意放开
|
||||||
|
CAD 轴吸收外参和重复性误差。现有单关节验证不能认证任意多轴组合。
|
||||||
|
|
||||||
|
采集稳定、求解成功、文件正确、实机精度通过应有独立状态。满足前三项仍不能
|
||||||
|
自动承诺第四项；本轮没有把软件通过表述为现场已能自动生成准确 URDF。
|
||||||
|
|
||||||
|
## 4. 本轮验证与证据
|
||||||
|
|
||||||
|
- 采集、预算、断点、重装隔离、局部范围和增量更新：46 passed，16.93 秒；
|
||||||
|
按测试约定未重复运行耗时完整整手虚拟时钟用例。
|
||||||
|
- 草稿、离线初始化、训练隔离及文件生成相关：39 passed，24.41 秒。
|
||||||
|
- 默认正式 CLI `--validate-only` 通过；它不能检测与现场安装的配置漂移。
|
||||||
|
- 初次测试命令覆盖了 ROS 的 PYTHONPATH，收集失败；修正环境后运行上述分组。
|
||||||
|
一次 `python -m runtime.runner` 未调用 main、没有实际校验，改为直接调用 CLI main 后确认通过。
|
||||||
|
- 真实冻结数据的端点敏感性检查已完成；既有 85 项测试通过没有覆盖并消除该漏洞。
|
||||||
|
|
||||||
|
本轮机器可读证据：
|
||||||
|
|
||||||
|
- [端点审计](../../calibration_output/software_review_20260923_current_flow/endpoint_audit.json)
|
||||||
|
- [配置差异、程序原稿与角度误差](../../calibration_output/software_review_20260923_current_flow/review_summary.json)
|
||||||
|
- [采集相关测试](../../calibration_output/software_review_20260923_current_flow/capture_tests.json)
|
||||||
|
- [离线相关测试](../../calibration_output/software_review_20260923_current_flow/offline_tests.json)
|
||||||
|
|
||||||
|
既有现场重复性证据：
|
||||||
|
`calibration_output/thumb_recapture_20260923_175829/live_comparison_20260923/repeatability.json`。
|
||||||
@@ -0,0 +1,148 @@
|
|||||||
|
# O30 右手离线修正逻辑复核（2026-09-23）
|
||||||
|
|
||||||
|
本轮使用封存会话 `splay_recapture_20260923_132346/O30_RIGHT_001/20260923_142201`。
|
||||||
|
原始数据未改写，没有 SDK 运动；训练轮 0/1 共 4769 帧，独立验证轮 3 共 2681 帧。
|
||||||
|
采集仍为 107/108 个方向完整。独立验证未参与参数拟合、候选选择或绑定判断。
|
||||||
|
|
||||||
|
## 已修正的问题
|
||||||
|
|
||||||
|
1. **标签所属连杆错误。** ID12–ID15 原配置为 `*_metacarpals`，程序因此认为 MCP/PIP 动作不会带动这些标签。
|
||||||
|
训练数据中四标签随 MCP 移动约 93–105 像素、随 PIP 移动约 37–49 像素，DIP 动作时仅约 0.2–0.3 像素。
|
||||||
|
小指对应 MCP/PIP 观测中掌部参考角点仅变化约 0.08/0.19 像素。
|
||||||
|
用户确认四标签都固定在中节或随中节运动的支架上，最终离线绑定为 `pinky_middle`、`ring_middle`、
|
||||||
|
`middle_middle`、`index_middle`。保持角色名称、ID、原始角点和 SDK 指令不变。
|
||||||
|
原始采集配置保留；本批求解必须使用下述新的离线配置，不能继续使用旧绑定配置。
|
||||||
|
2. **PnP 候选排序被误当成轨迹身份。** 实际 ID17 在 SDK 64→96 期间交换候选排序，固定取第 0/1 项会产生超过 40° 的虚假跳变。
|
||||||
|
草稿与正式求解的初始化现在复用候选连续关联，保留两个候选族，缺失的候选不复制成第二份证据。
|
||||||
|
初始化不用于授权唯一分支；原有在线严格连续性检查保持不变。最终拟合仍直接使用二维角点。
|
||||||
|
3. **扫描起点被上一段回程方向排除。** 初始化现在按正式扫描/分段归属纳入起点，四指补段只纳入实际运动的关节。
|
||||||
|
前向迟滞模型仍使用原始逐通道方向历史，不改写历史来伪造上行或下行。
|
||||||
|
4. **欠约束零位可能被漏判。** 当 Jacobian 行数小于列数时，保留完整右零空间，防止精简 SVD 遗漏不可观测方向。
|
||||||
|
5. **行程证据报告不准确。** 行程和基准样本只引用主视角有效可见标签及实际训练轮次，排除验证轮和无标签图像。
|
||||||
|
单训练轮草稿使用明确的规划选项；正式范围验收仍拒绝单轮证据。
|
||||||
|
6. **生成与验收混淆。** 终端分别显示全手参数生成、独立验证和草稿发布状态；结果清单记录模型绑定修正。
|
||||||
|
|
||||||
|
## 最终本批结果
|
||||||
|
|
||||||
|
目录：`calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_draft/`。
|
||||||
|
|
||||||
|
- `o30_right_draft.urdf` 与 `draft_calibration.json` 必须配套使用，SDK 指令不能直接作为 URDF 弧度。
|
||||||
|
- 11 个拟合零位、4 个用户定义的竖直侧摆零位、5 个保留 CAD 的末端零位；20 个拟合运动范围。
|
||||||
|
- 训练坐标 RMS：旧竖直草稿 5.5647 px → 本版 1.4867 px。
|
||||||
|
- JSON 与最终 URDF FK 最大差 5.55e-16；标准加载、原始文件哈希检查通过。
|
||||||
|
- 18/20 个关节的独立角度检查通过，包括四指全部关节及 `thumb_cmc_yaw`。
|
||||||
|
- `thumb_cmc_yaw` 独立角度 MAE 0.658°、P95 1.950°、最大 2.180°。
|
||||||
|
- `thumb_cmc_roll` 最大 10.306°、`thumb_mcp` 最大 4.262°，二者角度检查未通过。
|
||||||
|
- 全局最大测量重投影误差 16.47 px，最大角度置信界约 8.71°；全手精度仍未通过。
|
||||||
|
角度子项通过不等于零位绝对精度或任意多关节联动已认证。
|
||||||
|
- 没有更新 `latest_passed`，没有替换用户正在运行的 MuJoCo 模型。
|
||||||
|
|
||||||
|
`offline_motion_logic_review` 和 `offline_corrected_tag_links_draft` 是绑定核实过程中的对照产物，
|
||||||
|
已分别留下被最终中节绑定取代的说明，不能将其中的支座/近端绑定当成最终确认。
|
||||||
|
最终确认及像素依据见同级 `offline_confirmed_middle_tags.json`、`tag_motion_dependencies.json`。
|
||||||
|
|
||||||
|
## 只读离线重放
|
||||||
|
|
||||||
|
在项目根目录运行；输出目录必须是尚不存在的新目录：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/jazzy/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
OPENBLAS_NUM_THREADS=1 OMP_NUM_THREADS=1 ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_product.yaml \
|
||||||
|
--workspace /home/lxp/projects/linkerhand_retarget_ros2 \
|
||||||
|
--offline-raw calibration_output/splay_recapture_20260923_132346/O30_RIGHT_001/20260923_142201/raw_samples.jsonl \
|
||||||
|
--offline-draft --draft-upright-splay-zero \
|
||||||
|
--offline-output calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_replay
|
||||||
|
```
|
||||||
|
|
||||||
|
## 回归范围
|
||||||
|
|
||||||
|
新增 7 项集中回归均通过：真实候选换序、起点方向/四指补段、候选缺失及严格连续性门限、
|
||||||
|
欠约束零位、两个单训练轮草稿与正式拒绝边界、确认中节绑定的原始配置隔离及关节依赖。
|
||||||
|
其余受影响的草稿初始化、重复观测目标等价、JSON 导出、独立验证失败记录、CLI 和关闭控制链测试分组通过。
|
||||||
|
首次测试有 4 项因 shell 未加载 ROS 缺少 rclpy，2 项因新增夹具字段名错误失败；修正后对应项通过。
|
||||||
|
未恢复被外部删除的历史测试，也没有把离线回放当作实机验收。
|
||||||
|
|
||||||
|
|
||||||
|
## 新版仿真仍有拇指偏差的复核
|
||||||
|
|
||||||
|
只读订阅确认当前拇指 SDK 指令仍为 roll=55、yaw=181、MCP=122、IP=0；
|
||||||
|
实机反馈约为 56、176、120、4。仿真与新草稿文件哈希相同，使用配套指令映射。
|
||||||
|
新旧映射在当前 yaw 指令下输出分别为 79.164°、79.207°；新旧拇指末端 FK
|
||||||
|
仅差 0.219 mm 和 0.271°，因此上一轮修改并没有实质解决这个拇指姿态差异。
|
||||||
|
|
||||||
|
当前联合求解器固定 CAD 的关节轴线和拓扑，只拟合掌姿态、标签安装、零位和指令曲线。
|
||||||
|
训练轮中 MCP 轴的连续 PnP 假设与该模型轴存在偏差：正面 ID1 最接近的候选约 10.2°，
|
||||||
|
同机位末端标签约 9.94°，顶机位 ID17 约 13.09°。这些是几何诊断线索，不能排除相机
|
||||||
|
外参和候选歧义的影响，也不能直接将其作为允许写入 URDF 的轴修正。
|
||||||
|
静止标签的微小旋转不作为轴证据；原始候选与运动量完整保存在诊断 JSON 中。
|
||||||
|
|
||||||
|
原采集主要为单关节扫描，MCP 扫描将 yaw 保持在 SDK 80；当前 roll/yaw/MCP 的
|
||||||
|
55/181/122 组合不属于已直接观测的组合姿态。独立轮次中的角度子项通过并不认证
|
||||||
|
绝对关节轴/零位或任意组合运动。后续应优先验证拇指几何约束及组合姿态，不能用降低
|
||||||
|
误差阈值、照片手调 yaw 或再次导出同一参数代替修正。
|
||||||
|
|
||||||
|
诊断：`offline_confirmed_middle_tags_draft/thumb_current_pose_model_audit.json`。
|
||||||
|
本次仅诊断，未改求解参数、仿真模型或实机姿态。
|
||||||
|
|
||||||
|
|
||||||
|
## yaw 行程专项复核
|
||||||
|
|
||||||
|
本轮保持 CAD 转轴及全部拟合参数不变，只读训练角点、URDF/JSON 和当前 ROS 指令。
|
||||||
|
两个训练轮的正反扫描均有 SDK 0/255 对应的顶机位 ID17 有效图像，每个端点 3 帧。
|
||||||
|
SDK=255 的实际方向历史为 increasing（反向扫描起点也沿用此前的上行历史），没有把扫描
|
||||||
|
名称当成实际指令方向，更没有要求反馈必须等于 255 才认可发送端点。
|
||||||
|
|
||||||
|
当前输出曲线：SDK 80→32.97°、128→54.11°、181→79.16°、223→98.26°、255→113.39°。
|
||||||
|
URDF 上限为 113.3957°，高于原 CAD 约 107.25° 的名义范围。单独加载 MuJoCo 后对
|
||||||
|
181/223/255 与三种方向状态逐项执行映射和 mj_forward，qpos 与映射完全一致，未发生限位截断。
|
||||||
|
当前实机控制话题仍是 yaw=181，反馈约 176；这不能当成实机已到全行程终点。
|
||||||
|
|
||||||
|
保留两个连续 IPPE 假设、分别检查两训练轮正反向后，原始角点估计的相对总转角为
|
||||||
|
114.54°–115.15°，181 处约 80.00°–80.99°。与当前曲线相比，存在约 1°–2° 的偏小线索，
|
||||||
|
但这是姿态候选诊断，不能直接当成独立物理真值或据此放大行程。
|
||||||
|
当前零位修正为 −5.13°；它影响绝对朝向，不等于减少 5.13° 的相对行程。
|
||||||
|
明显组合姿态差异尚不能只由这约 1°–2° 的相对行程差解释。
|
||||||
|
|
||||||
|
原始诊断与完整逐停点数据：`offline_confirmed_middle_tags_draft/yaw_range_review.json`。
|
||||||
|
仿真核验：`linker_hand_mujoco_ros2/reports/yaw_mujoco_range_check.json`。
|
||||||
|
未发送实机动作，未改轴、放大曲线或替换任何模型。
|
||||||
|
|
||||||
|
|
||||||
|
## 当前中间指令的现场图像诊断
|
||||||
|
|
||||||
|
用户明确为同一中间指令下仿真偏小。为避免只对单关节历史数据推断组合姿态，
|
||||||
|
在现有 55/181/122/0 拇指指令保持不变时，短时读取三相机图像；没有机械手控制发布。
|
||||||
|
侧面可见 ID1/ID2，各 8 帧；原始文件单独存入 `current_thumb_pose_readonly`，不加入训练。
|
||||||
|
固定当前几何/零位/安装的图像角度诊断：仅调 yaw 最佳约 +2.16°；四关节共同解释时
|
||||||
|
约 roll +4.44°、yaw +0.82°、MCP −0.73°、IP +0.41°。残差与不确定度仍较大，
|
||||||
|
yaw 条件 3σ 约 5.96°，未包括共享几何不确定度，不能据此写入新修正。
|
||||||
|
这支持继续区分中段映射、绝对零位及组合姿态误差，不能直接扩大 URDF 上限。
|
||||||
|
详见 `current_thumb_pose_readonly/README.md` 与 `thumb_pose_measurement.json`。
|
||||||
|
|
||||||
|
## 纯 URDF 模式下分开复核零位与组合姿态
|
||||||
|
|
||||||
|
后续仿真按用户要求默认仅使用 URDF 线性映射。对已保存 55/181/122/0 组合图像重新诊断：
|
||||||
|
只调 yaw 为 +1.581°，同时拟合四拇指角度时 yaw 为 −0.503°；不能沿用旧曲线模式的 +2.160°。
|
||||||
|
训练两轮的条件零位复核为 yaw −5.331° / −4.907°，并未独立证明绝对零位正确。
|
||||||
|
|
||||||
|
发现最终原始记录存在相同输入冲突：训练轮 0 的四指侧摆，13:47:45 上行末端与 13:53:29
|
||||||
|
下行起点，控制指令、方向历史、相机内参及拇指反馈相同，掌部 ID0 中心只差 0.041 px，
|
||||||
|
拇指 ID1/ID2 中心分别差 6.264/15.468 px。7 帧与原始 JSONL 逐字段核对一致。
|
||||||
|
这些辅助拇指观测进入了零位求解；重复观测聚合虽保留原目标，却不能解决输入本身的不一致。
|
||||||
|
诊断性暂不使用四指侧摆任务辅助拇指观测后，yaw 零位 −5.130°→−5.018°，
|
||||||
|
指令 181 的拟合角度仅 79.164°→79.185°，因此此问题不足以解释全部 yaw 差异。
|
||||||
|
|
||||||
|
细节见采集目录 `current_thumb_pose_readonly/ZERO_AND_COMBINATION_REVIEW.md`；对应三个 JSON
|
||||||
|
保留条件、证据和敏感性结果。未改 CAD 转轴、原始数据、仿真 URDF、生产求解器或正式发布状态，
|
||||||
|
未使用第三轮拟合，也未发送实机动作。
|
||||||
|
|
||||||
|
### 对故障归因的修正
|
||||||
|
|
||||||
|
“观测有差异”不等于“离线实现有 bug”。再次核查发现，重复观测压缩完整保留组内平方误差；
|
||||||
|
7 帧实测数据的完整目标与压缩目标差值小于 1e-11 px²。现有草稿如实记录独立验收失败并禁止正式发布。
|
||||||
|
在现有确定性模型下采用最小二乘折中是合理实现，不能只凭不同采集段图像不一致就要求删除观测或修改拟合。
|
||||||
|
前面对“必须修正离线数据使用逻辑”的表述过强，应以此处结论为准：软件缺陷尚未证实；
|
||||||
|
相机视角、线性驱动近似、实际回位差异和固定几何模型的适用范围仍是待区分解释。
|
||||||
|
详见 `current_thumb_pose_readonly/hypothesis_reassessment.json`。本轮仅复核证据及更正说明，未修改生产代码或模型。
|
||||||
@@ -1,19 +1,38 @@
|
|||||||
# O30 轻量标定实现与验收边界
|
# O30 轻量标定实现与验收边界
|
||||||
|
|
||||||
2026-09-22。产品默认入口已切换为 `raw_joint_2_plus_1` / `raw_joint_images_v1`。软件流程已经接通；本文不是实机标定通过证明。
|
2026-09-23。O30 正式入口为 `raw_joint_2_plus_1` / `raw_joint_images_v2`。
|
||||||
|
采集结束与 URDF 验收是独立结果，软件验证不代表实机通过。
|
||||||
|
|
||||||
## 正式流程
|
## 正式采集流程
|
||||||
|
|
||||||
实测光学核验 → 一次启动检查 → 17 个任务、20 个关节连续两轮训练及一轮验证 → 安全归位 → 离线联合拟合 → 冻结独立验收 → JSON/URDF 原子发布。
|
启动检查 → 17 个任务、20 个关节连续两轮训练及一轮验证 → 当前失败方向立即补一次 → 安全归位 → 封存数据并关闭控制栈。
|
||||||
|
|
||||||
- 使用原运动执行器、避让路径、四指侧摆分段、9 个训练停点与双向稳态采样。108 个方向单元，训练轮身份 0、1，验证轮身份 3。验证轮编号 3 不表示执行四轮。
|
- 保留原运动路线、避让、四指侧摆分段、训练及验证停点和速度。108 个正式方向单元，训练轮身份 0、1，独立验证轮身份 3。
|
||||||
- 原始角点、合格 PnP 候选、同帧标签、指令、反馈、时间、方向及参考证据保存。所有实际发送指令另存 `raw_command_sent`，统计包括到位、避让、归位及重试。
|
- 移动标签只保存角点、身份、检测质量、相机参数、指令、反馈及时间。在线不运行移动标签 PnP、分支确认、父模型或辅助消歧；固定基准保护保留。每个任务只要求原指定主机位覆盖。
|
||||||
- 新入口不请求在线父模型、唯一移动标签分支、零位准备往返、辅助消歧或歧义回访。采集只检查同步、覆盖、端点、稳态和检测。
|
- 停点由稳态效果统一有界等待。中间指令步不再要求额外固定反馈位移；整个方向使用真实同向反馈推进保护，4 秒累计实际运动无推进即停止，计划内静止采样不消耗运动预算，切换停点不重置预算。
|
||||||
- 缺失单元移至队尾补采一次；再次不足记录缺项，继续其他任务，归位后报告失败。硬件停滞、反馈失联、参考移动仍保护停止。
|
- 按用户最新要求，端点以实际发送的 SDK 指令为准，不要求反馈等于 0/255。有效稳态图中同步记录的已发送端点指令也用于端点核验，不重复要求扫描阶段另有一张端点图；稳态图不混入运动覆盖、反馈行程或分箱统计。没有实际发到的目标值不能作为端点证据。
|
||||||
- 新断点只接收同协议、同受保护输入、同参考/可核验安装条件。中断的尝试消耗原补采预算；第一单元尚未通过也保留断点。拒绝不兼容数据，不静默重采。不可见安装参考明确拒绝恢复。
|
- 反馈失联、硬件故障、控制权冲突、越界、固定参考移动仍停止；避让和归位到位要求不变。按用户补充要求，停滞后等待 3 秒、从当前指令位置继续同一方向一次；再次停滞停止，不循环重发。
|
||||||
|
- 当前方向质量不足立即按原准备路径回到起点再采一次。仍不足记录缺项、继续下一方向；不队尾补采、不循环回访、不合并两个失败尝试。重启保留已消耗预算。
|
||||||
|
- 结束状态为 `CAPTURE_COMPLETE`，不是标定通过。`raw_capture_manifest.json` 独立记录流程结束、覆盖完整、尝试次数、缺项、配置和证据文件哈希。完整返回码为 0，缺项返回码为 3；两者都关闭控制栈。
|
||||||
|
- 在线不启动最终拟合。后续 `--offline-raw` 只读封存数据，单独保存输出，缺项或拟合失败都不触发运动。
|
||||||
|
- 离线可重新计算已完成方向的采集质量，并在 `acquisition_reassessment` 保存原失败原因和新检查结果；不改写原始记录、不拼接尝试，不接受中断或未完成的方向。断点恢复仍按原记录消耗补采次数。
|
||||||
|
- 新会话计划为 `capture_plan_v7_raw_joint`，立即补采语义与 v2 角点绑定。旧 v1 数据保持离线读取，禁止混入新会话续采。断点仍要求安装条件可核验。
|
||||||
|
|
||||||
|
## 历史实机卡点
|
||||||
|
|
||||||
|
`20260922_233830` 只有拇指旋转的 6/108 单元通过，拇指 MCP 指令 32、反馈 0 三次失败。
|
||||||
|
此前拇指旋转指令 223/255、反馈 189/195 也曾停滞。本次针对逐停点额外推进要求进行结构调整，
|
||||||
|
不能由这些历史日志单独推断真实死区、阻挡或反馈异常；当前实机结果须另行记录。
|
||||||
|
|
||||||
## 离线求解与发布
|
## 离线求解与发布
|
||||||
|
|
||||||
|
- 未认证草稿按任务接受至少一轮完整训练，并使用另一轮已通过采集质量检查的完整方向。
|
||||||
|
例如侧摆第二轮缺一方向时，第一轮完整仍可保留四指关节链；初始化取完整训练轮。
|
||||||
|
完整方向仍按原始质量门限复核，不拼接两次失败尝试，第三轮不补训练缺项。
|
||||||
|
逐任务覆盖、缺项和算法身份写入独立输出；采集完整性与正式发布规则不变。
|
||||||
|
- 草稿可显式复用同批数据的局部训练结果作初值，核对原始文件/配置身份与训练选择记录，
|
||||||
|
共享参数仍全部重新拟合；第三轮不用于初始化或选择。重复预测做加权等价合并，
|
||||||
|
保留组内散差和原始逐图误差，以减少内存和重复计算。草稿数值停止容差单独记录。
|
||||||
- `UrdfCommandImages` 复用共享图像优化入口，直接沿原 CAD 拓扑计算四指及非平行拇指。一个掌变换、每个实体标签一个共享安装；不引入各阶段独立六自由度变换。
|
- `UrdfCommandImages` 复用共享图像优化入口，直接沿原 CAD 拓扑计算四指及非平行拇指。一个掌变换、每个实体标签一个共享安装；不引入各阶段独立六自由度变换。
|
||||||
- 五根轴的方向与空间布局初始化掌坐标，子轴补充非平行约束；所有允许零位和双向单调指令映射在原始图像目标中联合求解。首次观测及行程端点不作为物理零位。
|
- 五根轴的方向与空间布局初始化掌坐标，子轴补充非平行约束；所有允许零位和双向单调指令映射在原始图像目标中联合求解。首次观测及行程端点不作为物理零位。
|
||||||
- 最多 64 个初始化，每个初始化的安装预优化与联合优化合计最多 200 次求值。正式后台进程沿用 600 秒上限；预算耗尽保留失败原因，不自动增加动作。
|
- 最多 64 个初始化，每个初始化的安装预优化与联合优化合计最多 200 次求值。正式后台进程沿用 600 秒上限；预算耗尽保留失败原因，不自动增加动作。
|
||||||
@@ -32,6 +51,18 @@ ros2 run linkerhand_calibration calibrate_hand \
|
|||||||
--no-resume --camera-optical-observations /绝对路径/optical_observations.json
|
--no-resume --camera-optical-observations /绝对路径/optical_observations.json
|
||||||
```
|
```
|
||||||
|
|
||||||
|
采集结束后，使用同一批只读数据离线修正；默认不发布：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--offline-raw /绝对路径/会话/raw_samples.jsonl \
|
||||||
|
--offline-output /绝对路径/新的离线结果目录
|
||||||
|
```
|
||||||
|
|
||||||
|
`--publish-offline` 仅在全部原精度与文件检查通过后才允许更新 `latest_passed`。
|
||||||
|
原始索引、压缩证据、导入证据和 `raw_capture_manifest.json` 须一起保留；不得编辑封存数据。
|
||||||
|
|
||||||
后续恢复省略 `--no-resume`，仍需提供当前安装条件下的光学观测。若安装参考不可核验，按拒绝原因处理，不能改哈希绕过。
|
后续恢复省略 `--no-resume`，仍需提供当前安装条件下的光学观测。若安装参考不可核验，按拒绝原因处理，不能改哈希绕过。
|
||||||
|
|
||||||
2026-09-22 用户明确要求复用已完成的相机标定：可将上述光学观测参数换为
|
2026-09-22 用户明确要求复用已完成的相机标定：可将上述光学观测参数换为
|
||||||
@@ -49,12 +80,129 @@ CameraInfo；日志及发布审计记录 `current_optical_verification: not_perf
|
|||||||
- `raw_joint_holdout.json`：每个保留候选的冻结验证。
|
- `raw_joint_holdout.json`：每个保留候选的冻结验证。
|
||||||
- `calibration_report.json` / `release_manifest.json`：协议、原始证据、安装参考、光学观测及候选集合身份；最终发布证据。
|
- `calibration_report.json` / `release_manifest.json`：协议、原始证据、安装参考、光学观测及候选集合身份；最终发布证据。
|
||||||
|
|
||||||
## 已验证与尚需实机完成
|
## 历史软件验证（2026-09-22）
|
||||||
|
|
||||||
- 全手运动效果测试覆盖 17 个任务、三轮正式往返、原避让和归位；实际 coordinator 虚拟时钟覆盖小指 18 个方向、全部已发送指令，无在线求解。
|
- 全手运动效果测试覆盖 17 个任务、三轮正式往返、原避让和归位；实际 coordinator 虚拟时钟覆盖小指 18 个方向、全部已发送指令，无在线求解。
|
||||||
- 有界队尾补采、跨恢复预算、首单元中断发现、协议/参考拒绝、固定参考保护、光学缺失时禁止启动均有测试。
|
- 当时的有界队尾补采、跨恢复预算、首单元中断发现、协议/参考拒绝、固定参考保护、光学缺失时禁止启动均有测试。当前 v2 已改为当前方向立即补一次。
|
||||||
- ID5、ID9、ID12 的真实历史双候选角点可作为采集数据；ID10 真实缺测不能变成完整扫描。
|
- ID5、ID9、ID12 的真实历史双候选角点可作为采集数据；ID10 真实缺测不能变成完整扫描。
|
||||||
- 独立 XML FK 真值：64 个初始化全部收敛，约 193 秒；32 个超过原图像门限，16 个被训练配对证据排除，剩余 16 个全部通过候选集合不确定度及第三轮验收。该批次生成合成 JSON/URDF，并通过标准 ROS 加载和最终文件 FK 回读。
|
- 独立 XML FK 真值：64 个初始化全部收敛，约 193 秒；32 个超过原图像门限，16 个被训练配对证据排除，剩余 16 个全部通过候选集合不确定度及第三轮验收。该批次生成合成 JSON/URDF，并通过标准 ROS 加载和最终文件 FK 回读。
|
||||||
- 合成产物位于 `calibration_output/software_review_20260922_raw_joint/known_truth/`，仅有软件测试指针 `synthetic_only_passed`；没有更新实机 `latest_passed`。
|
- 合成产物位于 `calibration_output/software_review_20260922_raw_joint/known_truth/`，仅有软件测试指针 `synthetic_only_passed`；没有更新实机 `latest_passed`。
|
||||||
- 该软件产物保留生成时的检验审计；当前更严格的 0.01 家族错误率复核另存 `current_training_selection.json`，保留候选集合完全相同，未重复运行优化器或改写原验收文件。
|
- 该软件产物保留生成时的检验审计；当前更严格的 0.01 家族错误率复核另存 `current_training_selection.json`，保留候选集合完全相同，未重复运行优化器或改写原验收文件。
|
||||||
- **尚需实机验收**：当前相机安装的实际光学核验、新协议首次全手采集、实机姿态对照。本次没有重新启动电机，不能将合成通过当作实际机械手的准确性证明。
|
- 当日没有重新启动电机；这些合成结果不能证明实际机械手的准确性。当前相机安装的光学核验和实机姿态对照仍须单独验收。
|
||||||
|
|
||||||
|
## 本次实施验证（2026-09-23）
|
||||||
|
|
||||||
|
软件完整 coordinator 模拟：108 方向均通过、没有补采，安全归位后封存且不再发送指令；
|
||||||
|
旧合成报告的 v1/v2 协议重建产生相同 URDF，原候选不确定度拒绝条件仍有效。
|
||||||
|
采集、立即补采、封存和只读离线检查详见 `TESTING.md`。
|
||||||
|
|
||||||
|
实机 `20260923_104837` 去程通过，拇指旋转回程指令降到 162 而反馈仍为 253，
|
||||||
|
累计实际运动无推进 4.005 秒后保护停止。角点记录亦未显示明显回程运动。
|
||||||
|
控制栈已退出，未重复强推、未自动归位；本次仅完成 1/108 方向，未生成通过的 URDF。
|
||||||
|
随后用户明确允许此类硬件停滞重试一次，已采用等待 3 秒后继续一次的策略；
|
||||||
|
新会话 `20260923_110651` 使用更新后的计划身份，不导入上述旧策略会话。
|
||||||
|
|
||||||
|
该新会话实际完成 35/108 个方向，原在线记录 29 个通过、6 个端点覆盖不足，
|
||||||
|
在小指 PIP 第三轮回程收到 `o30_sdk_identity_mismatch` 后停止。此时反馈约 29.81 Hz、
|
||||||
|
无活动硬件故障；通用“通信失联”提示不能证明物理断线。原日志没有保存异常身份报文，
|
||||||
|
无法确定具体不匹配字段；现已补充保留异常身份字段并单独显示身份错误，停止条件不变。
|
||||||
|
未触发硬件停滞重试，控制栈已退出，未完成归位和整手封存，未再次启动运动。
|
||||||
|
|
||||||
|
按最终端点规则对原始数据只读复核，35 个完整执行方向全部通过，原 6 个端点误判均消除；
|
||||||
|
还缺 73 个方向，禁止发布。复核报告位于
|
||||||
|
`calibration_output/software_review_20260923_lightweight/interrupted_capture_review.json`，
|
||||||
|
记录源数据哈希、算法身份、逐方向重算证据；源文件哈希未变化。
|
||||||
|
本次实机尚未完成全手流程和 URDF 精度验收。
|
||||||
|
|
||||||
|
同批数据随后做了真实拇指四关节局部图像拟合，发现 ID17 的安装连杆配置错误。
|
||||||
|
用户确认 ID17 位于随 MCP 弯曲的近端指节，仅顶机位可见；现改为 `thumb_proximal`，
|
||||||
|
同步产品配置哈希，保留机位、标签尺寸、任务和路线。原始记录与旧配置身份不改写。
|
||||||
|
仅训练数据的绑定对照使最大标签重投影 RMS 从 87.575 px 降到 8.047 px，
|
||||||
|
仍未通过原 1.5 px 门限，不能称为 URDF 修正成功或证明两轮足够。
|
||||||
|
具体结果与修改前配置快照见
|
||||||
|
`calibration_output/software_review_20260923_lightweight/thumb_fit_audit/README.md`。
|
||||||
|
本轮没有实机运动，没有使用第三轮选择或调整新绑定候选，也未发布 URDF。
|
||||||
|
|
||||||
|
## 用户允许先生成低精度修正文件（2026-09-23）
|
||||||
|
|
||||||
|
新增 `--offline-draft`，与正式精度认证分开。复用 `UrdfCommandImages` 的图像残差、
|
||||||
|
参数约束及优化器，仅为部分已采集关节增加轴线初始化；训练选解后冻结，第三轮仅报告。
|
||||||
|
能估计的零位与行程一起导出。单根手指的根零位与掌坐标存在等价变换，固定其 CAD
|
||||||
|
零位；其余估计零位须不落入图像雅可比的数值零空间。原已声明末端 CAD 零位仍明确列出。
|
||||||
|
不以收敛或多个初值一致冒充实际精度认证，草稿不更新 `latest_passed`。
|
||||||
|
|
||||||
|
实际旧数据生成于 `calibration_output/O30_RIGHT_001/draft_20260923_110651_v2/`：
|
||||||
|
拇指侧摆零位 -2.761403°，MCP 零位 -1.639888°；四关节行程分别为旋转 52.171947°、
|
||||||
|
侧摆 115.878573°、MCP 104.415960°、IP 82.065095°。其余缺测关节保持原 CAD。
|
||||||
|
完整结构 URDF 已通过 JSON 读回重建、最终 FK（最大差 4.44e-16）和标准 ROS 加载。
|
||||||
|
训练最大标签误差仍约 8.047 px，独立验证未通过原精度要求，报告如实保留。
|
||||||
|
原始数据哈希未变，没有实机运动。拇指旋转与 IP 的绝对零位未标为实测修正。
|
||||||
|
|
||||||
|
第一次导出发现有效稳态窗口中也存在无同步指令的原始帧，范围证据生成误读了这些帧。
|
||||||
|
已让正式/草稿共用范围证据函数，并统一通过既有 `fitting_frames` 选择同步完整记录；
|
||||||
|
保留全部原始日志，不把缺失指令填成零。失败 v1 结果保留，成功产物另存 v2。
|
||||||
|
|
||||||
|
## 全手草稿目标与采集后自动导出（历史，已由独立离线入口取代）
|
||||||
|
|
||||||
|
按用户要求，完整目标明确为 15 个非末端零位与全部 20 个运动范围；仅五指末端零位
|
||||||
|
保留 CAD。`correction_scope` 列出目标、实际估计与缺失关节，`full_target_generated`
|
||||||
|
只在目标均覆盖且文件校验通过后成立。旧局部拇指草稿不等于全手完成。
|
||||||
|
|
||||||
|
正常 CAPTURE_COMPLETE 后，启动器关闭 SDK/相机进程、销毁监控节点与自建 ROS 上下文，
|
||||||
|
刷新日志，然后才运行离线草稿。缺数据或求解失败都只保存结果，不进入补采或归位运动。
|
||||||
|
每次启动保存受保护 Profile 原始字节快照；后续仅修正安装连杆时可自动匹配旧会话身份。
|
||||||
|
|
||||||
|
上述自动拟合行为已在后续收敛中移除：默认采集现在归位、封存、关闭控制后直接退出，
|
||||||
|
仅写 `capture_result.json`；使用会话 `capture_product.yaml` 和 `--offline-raw` 单独拟合。
|
||||||
|
运动路线、2+1 分区、一次质量补采及硬件保护保持。配置快照一起进入封存清单。
|
||||||
|
|
||||||
|
独立 XML FK 图像软件检查使用当前 ID17 近端指节绑定、原正式采样网格和机位：
|
||||||
|
单个有界初始化收敛（12 次联合迭代），15 个零位参数均可观测，雅可比秩 463/463；
|
||||||
|
已知真值的最大零位差 0.001054°。复用这次拟合输出完整 URDF，确认 15 个 origin.rpy
|
||||||
|
实际改变、五个末端 origin 不变、20 个运动范围写入，最终 FK 最大差 5.55e-16、标准
|
||||||
|
ROS 加载通过。报告位于 `calibration_output/software_review_20260923_full_draft/`。
|
||||||
|
这是必要的全手软件检查，不是实机精度结论；没有以合成结果代替完整实机流程。
|
||||||
|
# 2026-09-23 ID10 调整与断点保留
|
||||||
|
|
||||||
|
实机 `20260923_120425` 保存 78 个通过方向（拇指、小指、无名指、中指），食指 MCP
|
||||||
|
六方向及 PIP 首方向两次都因 ID10 缺图失败，后续方向由操作者中止。原始日志、角点、
|
||||||
|
配置和哈希记录完整保留；这是中断会话，未宣称全手结束或归位。
|
||||||
|
|
||||||
|
操作者最初确认 ID10 仍在食指中节，后补充说明主要调整的是遮挡 ID10 的 ID15，且
|
||||||
|
ID15 仍在原来的侧摆支座；两者均未更换连杆。新增离线准备草稿断点入口，保留
|
||||||
|
连续通过前缀，并按 CAD 拓扑隔离受影响手指的历史观测；新数据采用新安装参数，
|
||||||
|
其他方向的补采预算不变。普通恢复及正式发布门限保持原义；该调整流程的未复核
|
||||||
|
安装明确标为草稿，不通过正式发布入口。
|
||||||
|
|
||||||
|
第一次启动恢复固定基准漂移不超过 0.325 px，但尚未测量的食指正面 ID15 归位位置
|
||||||
|
相对历史偏移 5.945 px，保护拒绝且未开始正式扫描。修正为整条受影响手指的历史
|
||||||
|
观测隔离，避免借用旧食指辅助观测绑定新安装，同时不放宽其他标签与固定基准门限。
|
||||||
|
|
||||||
|
## 2026-09-23 四指侧摆重采的恢复检查
|
||||||
|
|
||||||
|
操作者授权仅重采末项四指侧摆，在独立输出目录保存前 96 个方向的完整前缀，
|
||||||
|
旧侧摆全部记录保留在原会话，正式剩余计划为 12 个方向。两次实际启动在扫描前
|
||||||
|
被活动标签基准检查拒绝：`132507` 的 ID4/ID5 差异为 6.416/12.295 px，
|
||||||
|
`132938` 为 5.182/9.673 px；固定基准最大漂移分别为 0.264/0.219 px。
|
||||||
|
操作者确认小指标签与支架未调整。相同 SDK 指令及稳定反馈不能独立证明恢复了
|
||||||
|
相同物理姿态，因此在线不再将活动标签像素差直接判定为安装改变。
|
||||||
|
|
||||||
|
原始采集恢复仅将活动标签差异记录为 `requires_offline_review`，继续导入原证据
|
||||||
|
与尝试次数；不修改原始角点、旧基准或质量结论。固定基准和受保护输入检查仍为
|
||||||
|
阻断条件，失联、硬件故障及停滞保护不变。离线正式验收保留严格安装核验，并拒绝
|
||||||
|
带未复核安装记录的数据；草稿允许分析此类数据，但不得宣称精度或安装一致性通过。
|
||||||
|
本次只运行软件检查及已保存参考的只读回放，机械手未运动，后续由操作者启动采集。
|
||||||
|
|
||||||
|
## 2026-09-23 中指补段和四指联动的指令核对
|
||||||
|
|
||||||
|
`135113` 的中指向上补段（13:54:05.985–13:54:12.319）保存 625 条实际发送
|
||||||
|
记录，四指向量由 `[0,0,0,0]` 到 `[0,80,0,0]`；没有中指补段越过 80 的记录。
|
||||||
|
下一轮联动才继续增加中指指令。联动指令为 `[64,124,64,64]` 时，实际反馈为
|
||||||
|
`[59,120,0,55]`：无名指未动，而其他手指继续推进。操作者确认存在接触挤压，
|
||||||
|
因此补段范围正确不能证明同步联动的实体避让安全，现有 4 秒保护也不是同步到位保证。
|
||||||
|
|
||||||
|
状态增加子段名称及各通道声明范围，命令/反馈标明当前通道；实际下发日志增加
|
||||||
|
子段、方向、尝试次数和通道身份。这些只是显示及诊断改动，不改变运动路线、
|
||||||
|
速度或保护预算，不宣称解决接触问题。三轮/归位/立即补采相关检查 3 passed
|
||||||
|
（1.48 秒），另核对三轮全部 12 个方向目标及子段显示。未启动机械手，未操作
|
||||||
|
操作者运行的范围检查 SDK。实际指令审计保存在该会话的 `middle_lower_command_audit.json`。
|
||||||
|
|||||||
@@ -0,0 +1,83 @@
|
|||||||
|
# O30 剩余精度问题：封存数据定位结果
|
||||||
|
|
||||||
|
2026-09-24。只读复核旧外参全手 `20260923_142201` 和新外参拇指
|
||||||
|
`20260923_183138`，使用各自封存配置和本轮已冻结的参数。诊断脚本与 JSON 报告位于
|
||||||
|
`calibration_output/software_fix_20260923_auto_urdf_v2/`。没有修改原始证据、候选 JSON/URDF、
|
||||||
|
采集路线或精度门限；没有硬件运动。本轮新增的只是只读诊断脚本和这份报告。
|
||||||
|
|
||||||
|
## 已确认的三个独立问题
|
||||||
|
|
||||||
|
**字节到关节角不能仅靠 URDF 上下限线性换算。**逐字节比较程序生成的 JSON 曲线
|
||||||
|
与同一 URDF 的线性插值：拇指 yaw 上行在字节 96 的差为 3.167°，IP 上行在字节 128
|
||||||
|
的差为 2.273°；两端差均约为 0。这解释了为什么扩大 URDF 上限不能修正中间行程。
|
||||||
|
物理几何仍由 URDF 提供，仿真/ROS 的字节输入必须先经认证 JSON 的对应方向曲线
|
||||||
|
映射为关节弧度。当前 JSON 精度未认证，不能通过改变加载标志直接用于正式控制。
|
||||||
|
逐字节结果见 `driver_gap.json`。
|
||||||
|
|
||||||
|
**新拇指 roll 的正面图像有重复出现的空间残差。**冻结拟合在训练轮的 thumb IP
|
||||||
|
标签最大前向误差 7.238 px，验证轮独立测角后最大 7.194 px。训练轮字节 0 的
|
||||||
|
thumb IP 标签误差中位数约 2.093 px，字节 255 约 7.114 px；MCP 与 IP 标签在
|
||||||
|
高角度的误差方向接近。高误差在训练和验证轮均出现，继续增加同一模型的优化
|
||||||
|
迭代没有足够依据。旧全手的最坏观测另在顶部 thumb yaw 标签、thumb MCP 任务中，
|
||||||
|
最大 16.468 px，不应混作新拇指 roll 的误差。
|
||||||
|
|
||||||
|
受限的几何假设检验：冻结其他参数后，仅用新批次训练轮拟合一个相对字节 0
|
||||||
|
的 CMC roll 转轴平移，得到局部 y≈+0.052 mm、z≈−1.969 mm。用该值回放独立
|
||||||
|
验证轮，未做每图角度修正的 thumb IP 最大前向误差从 7.672 px 降到 5.261 px，
|
||||||
|
MCP 从 6.793 px 降到 3.057 px；依旧未通过 1.5 px 图像门限。
|
||||||
|
这只能说明“共同上游几何变化”可解释一部分误差，不能区分真实转轴位置、
|
||||||
|
标签随运动的位移或受力形变，更不能据此修改 URDF 的 `origin.xyz`。
|
||||||
|
试探代码及结果见 `probe_thumb_pivot.py` / `thumb_pivot_probe.json`。
|
||||||
|
|
||||||
|
**旧全手参考在新批次的辅助食指端点存在偏差。**新拇指 roll 任务明确令食指侧摆
|
||||||
|
通道保持字节 0，其他拇指任务为 255。该食指标签在验证轮字节 0 的误差中位数
|
||||||
|
5.192 px，在 255 时为 1.158 px。只用新批次训练图像试探一个 +0.974° 的
|
||||||
|
食指字节 0 临时角度修正，验证轮中位误差降到 1.954 px，仍高于门限。
|
||||||
|
这支持旧参考的端点角度有偏差，但不支持用单个辅助保持姿态改写整条四指曲线。
|
||||||
|
旧全手原始采集仍少一个训练方向（107/108），旧参考系统误差也没有完整传入
|
||||||
|
新拇指不确定度。试探代码及结果见 `probe_reference.py` / `reference_probe.json`。
|
||||||
|
|
||||||
|
## 观测条件与下一次采集
|
||||||
|
|
||||||
|
新批次拇指 roll 的三轮稳态窗口中，顶部相机分别保存了 32、29、37 帧，
|
||||||
|
**全部只有固定 `top_base` 标签**；随拇指运动的顶部 `thumb_cmc_yaw` 标签没有
|
||||||
|
一帧有效观测。正面相机同时看到了 thumb MCP/IP 标签。这足以发现投影误差，
|
||||||
|
却不足以从独立视角确认 CMC roll 转轴的三维位置。
|
||||||
|
逐帧来源计数见 `thumb_roll_visibility.json`。
|
||||||
|
|
||||||
|
下一批采集前的顺序：
|
||||||
|
|
||||||
|
1. 在当前拇指 roll 全程的 0、各中间节点和 255，确认顶部或另一独立视角持续
|
||||||
|
看到**随拇指运动的标签**，并且正面仍可看到 MCP/IP 与固定参考。先用静态
|
||||||
|
预览确认可见性，避免整手运动后才发现动态标签缺失。ID17 当前固定在
|
||||||
|
`thumb_proximal`；若能刚性固定在 yaw 子连杆 `thumb_metacarpals_base2` 并保持
|
||||||
|
全行程可见，可减少 MCP 运动对 roll/yaw 观测的混合，但必须更新标签连杆绑定。
|
||||||
|
移动相机需要重测外参；只移动标签需要重估其安装位姿。两者均需建立新批次，
|
||||||
|
不能沿用旧标签安装证据。
|
||||||
|
侧面机位当前仅声明四指运动标签；若用于 yaw，需要侧面实际可见的拇指运动
|
||||||
|
标签与固定参考，并接入 Profile 与离线求解。若侧面与顶部共用物理 ID17，
|
||||||
|
还需让求解器保存各机位观测并共用同一个标签安装参数，不能将两个机位当成
|
||||||
|
两个独立安装。
|
||||||
|
2. 对拇指 roll 的端点分别核对两轮方向、反馈稳定、标签角点和刚性；检查字节 255
|
||||||
|
是否存在遮挡、标签与手指碰撞或安装件受力。当前同一误差在训练/验证重复出现，
|
||||||
|
应优先排查物理条件和 CAD 转轴，再决定是否扩大几何拟合参数。
|
||||||
|
3. 若目标是**正式全手 URDF**，在同一有效外参和安装条件下采完整 108 方向的
|
||||||
|
全手 2＋1 数据。旧批次缺失方向不能用新外参的几帧直接拼成已完成旧批次。
|
||||||
|
新拇指 24/24 可作诊断和拟合初值，但不替代同批全手独立认证。
|
||||||
|
4. 用新批数据先冻结训练解，再独立验收图像、零位、端点和整段曲线；另行验证
|
||||||
|
JSON 曲线驱动的 URDF。若确有多视角证据支持转轴位置偏差，再扩展共同几何
|
||||||
|
模型和 URDF `origin.xyz` 的授权字段，并验证 JSON 回读和最终文件 FK。
|
||||||
|
当前求解器只导出零位角与限位，增加图像本身不会自动修改转轴位置。
|
||||||
|
|
||||||
|
新批次 yaw 字节 16 下降方向的单张正面图像出现 3σ≈1.733° 的测角不确定度。
|
||||||
|
相同保持指令与反馈下，顶部也有三张动态标签图像。将该保持窗口的正面与顶部
|
||||||
|
观测作为同一角度的**诊断**时，3σ≈0.230°；时间跨度约 0.242 s。
|
||||||
|
这表明当前逐张单视角测角方式可能高估该稳态窗口的不确定度，但重复图像存在
|
||||||
|
共同几何误差，不能简单按独立噪声缩小正式置信区间。联合多视角验收须显式
|
||||||
|
处理曝光同步与相关误差；即便解决这项不确定度，前述 7 px 图像残差仍会拒绝发布。
|
||||||
|
数据见 `multiview_angle_probe.json`。
|
||||||
|
|
||||||
|
当前程序生成候选继续保持 `accuracy_passed=false`、`publication_allowed=false`；
|
||||||
|
仿真默认和 `latest_passed` 均未切换。保存的报告保留了每个受影响标签、任务、
|
||||||
|
方向和样本时间戳，见 `old_residual_diagnostics.json` 与
|
||||||
|
`new_residual_diagnostics.json`，便于新批次做同项对照。
|
||||||
@@ -13,9 +13,18 @@ O30 右手的构建、相机外参重标和完整操作说明见 [O30 右手标
|
|||||||
O30 当前使用反馈判断运动、稳定及零位恢复；允许指令与反馈存在固定偏差。最终 JSON 仍为指令→视觉角度，反馈映射只作诊断。
|
O30 当前使用反馈判断运动、稳定及零位恢复；允许指令与反馈存在固定偏差。最终 JSON 仍为指令→视觉角度，反馈映射只作诊断。
|
||||||
测试的分层、选择范围与耗时记录见 [测试说明](TESTING.md)。
|
测试的分层、选择范围与耗时记录见 [测试说明](TESTING.md)。
|
||||||
|
|
||||||
|
## O30 当前默认：轻量采集，离线修正
|
||||||
|
|
||||||
|
默认 `raw_joint_2_plus_1` / `raw_joint_images_v2`：移动标签仅保存二维角点，
|
||||||
|
每任务两轮训练加一轮验证；当前失败方向立即补采一次，仍失败记录缺项并继续。
|
||||||
|
采完归位、封存数据、关闭控制栈，终态 `CAPTURE_COMPLETE` 不等于 URDF 通过。
|
||||||
|
后续使用 `--offline-raw` 读取同一批数据修正，失败不再触发运动。
|
||||||
|
启动、离线命令和协议边界见 [O30 轻量标定说明](O30_RAW_PROTOCOL_IMPLEMENTATION.md)。
|
||||||
|
以下分阶段和自适应章节为历史显式试验入口，不是 O30 当前默认流程。
|
||||||
|
|
||||||
## O30 分阶段训练试验
|
## O30 分阶段训练试验
|
||||||
|
|
||||||
新增显式选项 `--training-policy staged_2_to_3`，默认仍为 `fixed`。Tag 数量、编号、尺寸、位置、
|
新增显式选项 `--training-policy staged_2_to_3`，该历史策略以 `fixed` 为对照。Tag 数量、编号、尺寸、位置、
|
||||||
相机布局、完整网格、运动速度和验收门限不变。新策略按原任务顺序执行全手轮次 0、全手轮次 1，
|
相机布局、完整网格、运动速度和验收门限不变。新策略按原任务顺序执行全手轮次 0、全手轮次 1，
|
||||||
进行训练检查；必要时仅集中补一次轮次 2，随后冻结整手模型，最后采集独立验证轮次 3。
|
进行训练检查；必要时仅集中补一次轮次 2，随后冻结整手模型，最后采集独立验证轮次 3。
|
||||||
局部零位补采包含同轮几何依赖，分段任务整体补采，共享掌部问题扩大到全手。补采后仍不合格即失败。
|
局部零位补采包含同轮几何依赖，分段任务整体补采，共享掌部问题扩大到全手。补采后仍不合格即失败。
|
||||||
@@ -47,7 +56,7 @@ JSON→URDF 重建、文件与像素验收、成对发布、失败不更新通
|
|||||||
|
|
||||||
## O30 自适应训练试验
|
## O30 自适应训练试验
|
||||||
|
|
||||||
默认仍为 `fixed`：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后
|
该历史策略以 `fixed` 为对照：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后
|
||||||
检查覆盖、稳态网格、双向重复性和可计算的几何零位质量；证据不足补第三轮，第三轮仍不合格
|
检查覆盖、稳态网格、双向重复性和可计算的几何零位质量；证据不足补第三轮，第三轮仍不合格
|
||||||
就失败。每个方向的缺样恢复仍最多同速重扫一次，准备缺样仍最多局部恢复一次。
|
就失败。每个方向的缺样恢复仍最多同速重扫一次，准备缺样仍最多局部恢复一次。
|
||||||
速度、双向运动顺序、节点、准备路径及最终精度门限均保留。
|
速度、双向运动顺序、节点、准备路径及最终精度门限均保留。
|
||||||
@@ -840,3 +849,97 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
|
|||||||
主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接
|
主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接
|
||||||
投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 SDK 条件不一致时拒绝发布。
|
投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 SDK 条件不一致时拒绝发布。
|
||||||
这不表示联合求解或外参自校准已经通过验证。
|
这不表示联合求解或外参自校准已经通过验证。
|
||||||
|
|
||||||
|
## 低精度离线修正草稿（O30）
|
||||||
|
|
||||||
|
最新程序修复、两批实采数据验证及未通过的精度项目见
|
||||||
|
[O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
当前目标是 **15 个非末端关节零位＋20 个关节运动范围**；拇指 IP 与四指 DIP 的五个
|
||||||
|
末端零位保留 CAD，其运动范围仍由数据拟合。完整全手数据可提供局部拇指缺少的掌坐标
|
||||||
|
约束；只采部分任务时，不能将局部输出当作上述全手目标已完成。
|
||||||
|
|
||||||
|
O30 原始采集正常结束后，启动器完成归位封存、关闭 SDK/相机及控制上下文后退出。
|
||||||
|
`capture_result.json` 只报告采集结果，完整返回 0、缺项返回 3，不自动拟合。
|
||||||
|
每个新会话保存可供离线使用的 `capture_product.yaml`、Profile、源 URDF、内外参和
|
||||||
|
标签配置快照，并随原始证据封存。离线失败不改变采集状态，也不会再次运动。
|
||||||
|
|
||||||
|
也可手动从已完成任务生成完整结构的修正 URDF，再优化精度：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config /绝对路径/会话/capture_product.yaml \
|
||||||
|
--offline-raw /绝对路径/会话/raw_samples.jsonl \
|
||||||
|
--offline-draft --offline-output /绝对路径/新的草稿目录
|
||||||
|
```
|
||||||
|
|
||||||
|
草稿复用现有图像拟合器、零位约束和 URDF 写入器。训练数据估计可观测的零位及
|
||||||
|
双向指令曲线，验证数据仅报告误差；超过原精度门限仍可生成文件。
|
||||||
|
未独立观测的反向换向端点采用有来源的连续性约束，不保留可任意改变行程的自由参数；
|
||||||
|
约束写入 `motion_range_evidence.command_support`。缺少端点依据时拒绝生成行程。
|
||||||
|
`draft_urdf_linear_holdout.json` 单独报告纯 URDF 上下限线性驱动的误差；曲线精度
|
||||||
|
不能替代线性驱动验收，未通过精度验收的草稿不更新正式发布指针。
|
||||||
|
每个任务至少有一轮完整训练即可参与草稿求解，另一训练轮已通过采集质量检查的
|
||||||
|
完整方向也保留；不拼接失败尝试，不用第三轮补训练缺项。初始化使用该任务已完整的
|
||||||
|
训练轮，逐任务覆盖与选择理由写入 `draft_result.json` 的 `training_selection`。
|
||||||
|
父关节观测约束仍保留；草稿可覆盖全手与原始采集完整是两个独立结果。
|
||||||
|
缺测关节保留原 CAD，单根手指无法观测的根零位及已声明的 CAD 末端零位单独列出。
|
||||||
|
不从数值不可观测的自由度导出任意零位。JSON 读回、字段授权、运动范围、最终 FK、
|
||||||
|
标准 URDF 加载和来源哈希检查仍必须通过。
|
||||||
|
|
||||||
|
离线模型纠错可使用单独的 Profile/Product 文件，只允许修改标签连杆绑定和
|
||||||
|
`sdk_to_joint_direction`；原始会话仍按保存的 `capture_profile.yaml` 校验。
|
||||||
|
基准 SDK 指令、通道、采样路线和速度不允许借此更改。新旧方向及配置哈希写入
|
||||||
|
`draft_result.json`，不重写原始记录，也不影响在线控制。
|
||||||
|
求解前以训练轮的双向运动和两种平面姿态候选检查 CAD 平行根轴的方向一致性；
|
||||||
|
所有候选均反向时拒绝拟合，记录 `draft_sdk_to_urdf_direction_conflict`。
|
||||||
|
不能靠零位补偿错误的 SDK/URDF 方向。文件能加载及参数数量齐全，也不等于物理零位正确。
|
||||||
|
|
||||||
|
现场明确确认四指在采集基准指令下竖直时，离线命令可增加
|
||||||
|
`--draft-upright-splay-zero`。此选项先检查源 CAD 的四指零位为掌坐标竖直，
|
||||||
|
再从求解变量中移除四个侧摆零位，重新拟合其他零位与全部指令曲线；不是导出后改姿态。
|
||||||
|
JSON 将这四项标为 `operator_defined`，分别统计 11 个拟合零位、4 个用户定义零位及
|
||||||
|
5 个 CAD 末端零位，不能把四个定义值当作独立测量。原始基准指令、采集配置和记录不变；
|
||||||
|
该选项不能进入在线采集或正式发布。
|
||||||
|
|
||||||
|
同批数据已有局部草稿时，可增加 `--draft-initialization-from /绝对路径/已有草稿目录`。
|
||||||
|
入口核对原始数据及配置身份，只读取训练选出的起始姿态和曲线；所有待求参数仍在
|
||||||
|
全手训练图像中联合调整。不同批次、使用验证选择的结果不能作为该初值。
|
||||||
|
其文件哈希和用途写入新报告，不覆盖已有草稿。
|
||||||
|
|
||||||
|
全手草稿按相同标签、相同祖先关节指令/方向及相机参数合并重复预测，使用样本数权重
|
||||||
|
及组内散差保持原始最小二乘目标、梯度与信息矩阵；误差报告仍逐张原始图像计算。
|
||||||
|
草稿数值停止 `ftol=1e-5`，正式求解保持 `1e-9`；这不改变任何精度、可观测性或
|
||||||
|
发布门限，草稿无论数值收敛与否都不能自动更新 `latest_passed`。
|
||||||
|
草稿最多保留四个收敛初值的候选并按训练误差选择，报告已计算初值数量及是否穷尽；
|
||||||
|
不以这个有限搜索声称已排除全部几何解。正式候选搜索与发布验收保持原规则。
|
||||||
|
|
||||||
|
输出 `o30_right_draft.urdf`、`draft_calibration.json`、`draft_result.json` 及训练/验证报告。
|
||||||
|
草稿 JSON 使用独立格式，不作为正式控制映射或精度证书自动加载；URDF 可标准加载。
|
||||||
|
该入口允许读取中断会话的完整方向，不启动 SDK、不补采、不更新 `latest_passed`。
|
||||||
|
`--offline-draft` 不能与 `--publish-offline` 同用，正式精度验收入口保持不变。
|
||||||
|
|
||||||
|
若旧数据的标签连杆配置有误，提供 `--capture-profile /绝对路径/原Profile.yaml`；
|
||||||
|
新会话已保存 `capture_profile.yaml` 时会自动使用该快照。
|
||||||
|
先核对原 Profile 与会话哈希，再仅允许当前 Profile 修正标签安装连杆。
|
||||||
|
标签身份、尺寸、运动策略、相机与 CAD 来源不允许借此改变，原始记录保持只读。
|
||||||
|
|
||||||
|
采集中途重新安装移动标签时，不能直接沿用该标签原来的安装参数。O30 草稿流程可先
|
||||||
|
离线建立新断点，再按普通断点恢复：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--prepare-remount-from /绝对路径/原会话 --remounted-tag-id 10
|
||||||
|
```
|
||||||
|
|
||||||
|
此步骤不启动相机或 SDK。只允许保留调整标签之前连续通过的任务前缀；原会话不修改，
|
||||||
|
新断点按 CAD 拓扑排除受影响手指的全部旧标签角点，从首个受影响任务重新采集，
|
||||||
|
其他方向的补采次数保持原值。未开始新扫描的准备断点可扩大排除范围；已开始的尝试
|
||||||
|
不能借此重新获得预算。
|
||||||
|
原会话保存一次性后继指针，后续重启使用普通恢复，不能再次从旧安装重置尝试次数。
|
||||||
|
原始角点续采时，活动标签在同一 SDK 基准指令下的像素/反馈差异及不可见情况
|
||||||
|
列为“待离线复核”，不将回位姿态差异直接判成标签重装，不据此暂停采集。
|
||||||
|
固定基准、配置身份及运动保护仍须通过。前后安装参考、差异和原补采预算均保留；
|
||||||
|
未核实差异只允许进入未认证修正草稿，正式发布仍拒绝未复核的安装关系。
|
||||||
|
标签必须仍在原配置连杆上；若移到了其他指节，先修正连杆绑定。
|
||||||
|
|||||||
@@ -1,5 +1,85 @@
|
|||||||
# 标定测试的执行范围
|
# 标定测试的执行范围
|
||||||
|
|
||||||
|
2026-09-24 O30 剩余误差诊断：只读两批封存数据及已冻结参数，旧/新独立验证图像
|
||||||
|
分别 2681/516 帧；按机位/标签/任务/指令拆分残差，受限试探仅用训练图像选参数、
|
||||||
|
验证轮报告结果。顶部相机在新拇指 roll 三轮只见固定标签；跨批食指端点偏差及
|
||||||
|
拇指 roll 高角度重复性残差已定位。JSON/URDF 逐字节差、来源哈希和所有试探结果
|
||||||
|
记录于 [O30_REMAINING_ISSUES.md](O30_REMAINING_ISSUES.md)。未修改生产拟合、采集、
|
||||||
|
门限或已生成候选；诊断结果不计作正式精度测试通过，也未驱动硬件。
|
||||||
|
|
||||||
|
2026-09-23 自动 URDF 修复：采集/局部/重装组 43 passed（17.06 秒），离线草稿及
|
||||||
|
换向端点组 43 passed（34.18 秒），更新审计 3 passed（0.33 秒）。完整真实
|
||||||
|
coordinator 的虚拟时钟 108 方向集成 1 passed（97.21 秒），用于核对完整调度和
|
||||||
|
训练/验证隔离，未启动硬件。最后补齐快照 mesh 与封存路径检查后，仅复查受影响
|
||||||
|
4 项，4 passed（2.38 秒）；此组与前面重叠，不重复累计。
|
||||||
|
早期失败包括旧夹具仍使用已纠正的标签/方向配置、近端插值掩盖未观测换向端点；
|
||||||
|
分别修正历史场景声明、改用有来源的端点连续性约束后通过，没有降低验收门限。
|
||||||
|
旧外参全手与新外参拇指均按自身配置回放，训练重新收敛，JSON 重建 URDF 字节
|
||||||
|
一致，FK 最大差 6.66e-16，ROS/MuJoCo 加载通过；独立精度仍未通过，未发布。
|
||||||
|
证据、物理精度限制及候选路径见 [O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
2026-09-23 离线逻辑复核：复用连续候选关联替代逐帧 PnP 排序，扫描起点按扫描身份纳入；
|
||||||
|
补全欠约束 SVD 零空间，范围证据记录实际可见训练样本和轮次，单轮仅允许未认证草稿。
|
||||||
|
新增 7 项集中回归通过；其余受影响项分组通过。首次 24 passed/4 failed/2 errors（22.21 秒）
|
||||||
|
中的失败分别为未 source ROS 和新增夹具字段名错误；修正后对应及 CLI 10 passed（2.13 秒）。
|
||||||
|
范围边界组 5 passed（1.20 秒），中节绑定/历史数据隔离组 2 passed（1.12 秒）。
|
||||||
|
用户确认 ID12–ID15 随中节运动后，新离线配置将四标签绑定至 `*_middle`；封存数据回放生成
|
||||||
|
`offline_confirmed_middle_tags_draft`，训练坐标 RMS 1.4867 px，最终 FK 差 5.55e-16。
|
||||||
|
四指角度子项通过，但拇指 roll/MCP 及全局图像/不确定度未通过；未发布，未控制实机。
|
||||||
|
详细问题、证据和复现命令见 [O30_OFFLINE_LOGIC_REVIEW.md](O30_OFFLINE_LOGIC_REVIEW.md)。
|
||||||
|
|
||||||
|
2026-09-23 四指侧摆竖直基准：新增显式离线选项 `--draft-upright-splay-zero`，
|
||||||
|
四项作为用户定义基准移出自由零位参数，全部行程与其余零位重新拟合。
|
||||||
|
新增/CLI 用例 6 passed（13.04 秒），相关范围、缺项、导出及生命周期用例
|
||||||
|
18 passed（16.88 秒）。`offline_upright_zero_draft` 最终 URDF 的四指正面侧偏均为
|
||||||
|
0°；JSON/FK 最大差 8.89e-16，标准加载与来源哈希检查通过。
|
||||||
|
报告区分 11 个拟合零位、4 个用户定义零位、5 个 CAD 末端零位和 20 个拟合范围。
|
||||||
|
本次独立验证已算出角度误差，但整手精度仍未通过，未更新 latest_passed。
|
||||||
|
原始数据保持不变，未进行硬件运动；上一版侧摆偏移 −6.6° 至 −9.4° 的草稿已附拒绝说明。
|
||||||
|
|
||||||
|
2026-09-23 物理零位复核：旧 `offline_full_hand_draft` 因四指 SDK/URDF 方向反转而
|
||||||
|
错误估计约 −20° MCP 零位，已附拒绝说明，不能将其文件检查通过理解为零位正确。
|
||||||
|
独立离线配置纠正方向后，`offline_corrected_zero_draft_v2` 的四指 MCP 为
|
||||||
|
+0.385°、−0.306°、−0.690°、−1.475°，没有强制置零；JSON/URDF FK 最大差
|
||||||
|
7.78e-16、标准加载及原始数据哈希检查通过。独立验证求解器仍报 IndexError，
|
||||||
|
保留 traceback，精度/发布均为 False；没有硬件动作。
|
||||||
|
本次 `test_raw_draft.py` 27 passed（30.15 秒），新增实采角点方向回归、离线模型
|
||||||
|
修订边界及验证求解异常不冒充精度通过的检查。原始数据仍为 107/108 单元。
|
||||||
|
|
||||||
|
全手草稿候选搜索收敛为最多四个收敛结果，明确记录非穷尽；相关数值不可观测拒绝、
|
||||||
|
候选预算和训练初值契约 3 passed、21 deselected（0.90 秒）。
|
||||||
|
实机封存数据 `20260923_142201` 已生成全手草稿于
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/offline_full_hand_draft/`：
|
||||||
|
15 个非末端零位、20 个范围，五个末端零位保留 CAD；原始采集仍为 107/108。
|
||||||
|
复用已计算且收敛的同目标训练候选，重算原始像素误差确认一致，再运行正常草稿验收；
|
||||||
|
缓存文件、脚本和来源哈希见 `numerical_fit_reuse.json`。JSON/最终 URDF FK 最大差
|
||||||
|
6.66e-16，标准加载通过。独立验证角度求解失败，四个零位接近 ±20° 模型边界；
|
||||||
|
精度和发布均未通过，`latest_passed` 未更新。没有硬件动作。
|
||||||
|
|
||||||
|
2026-09-23 同批数据全手离线求解支持：最终 `test_raw_draft.py` 23 passed（14.47 秒）。
|
||||||
|
新增重复观测合并的原始目标、梯度和信息矩阵等价检查，原始图像误差仍逐帧报告；
|
||||||
|
训练初值拒绝不同数据批次或使用验证选择的结果，相关 CLI 不能进入在线运动。
|
||||||
|
首次等价测试的近零梯度逐元素断言受浮点消减影响（最大绝对差 4.62e-7、全梯度相对差
|
||||||
|
3.32e-13），改为完整梯度范数误差小于 1e-11 后通过，未改模型或采集精度门限。
|
||||||
|
试验过的自适应参数缩放未在合成全手收敛检查中通过，已撤回；最终沿用原参数缩放。
|
||||||
|
仅未认证草稿的数值停止容差为 1e-5，正式求解默认值及发布检查不变。
|
||||||
|
|
||||||
|
2026-09-23 离线草稿支持单轮完整训练：`test_raw_draft.py` 20 passed（8.42 秒）。
|
||||||
|
覆盖任一训练轮缺四指联动上行时仍保留全手求解范围、初始化选择另一完整训练轮、
|
||||||
|
两轮互补缺项或完整验证轮不能冒充完整训练轮，以及原有不可观测零位拒绝、JSON
|
||||||
|
重建和只读离线生命周期契约。未改变正式采集完整性或发布门限；未恢复历史测试。
|
||||||
|
实机数据的实际离线求解另行记录，不把上述软件测试当作精度验收。
|
||||||
|
|
||||||
|
2026-09-23 原始续采的活动标签基准检查：12 passed、30 deselected（1.37 秒）。
|
||||||
|
覆盖活动标签像素/反馈差异记录后继续导入、缺少基准记录的离线复核标记、固定基准
|
||||||
|
移动仍拒绝、非有限反馈拒绝、正式发布拒绝未复核安装、草稿仍检查来源完整性，
|
||||||
|
以及现有标签重装/次数持久化契约。真实 coordinator 检查确认没有因活动标签
|
||||||
|
基准差异暂停，且将未复核状态写入日志。未启动 SDK 或相机。
|
||||||
|
两份实机停止记录 `132507` / `132938` 的固定参考与 96 个方向断点只读复核位于
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/software_review_resume_reference.json`。
|
||||||
|
不重判旧采样质量、不修改角点；下一任务为四指侧摆第 1 轮正向联动。
|
||||||
|
这只证明恢复检查和任务位置正确，不代表后续实机运动或 URDF 精度验收通过。
|
||||||
|
|
||||||
2026-09-22 正式 2＋1 局部失败调度收敛：新增 test_taskwise_recovery.py，覆盖
|
2026-09-22 正式 2＋1 局部失败调度收敛：新增 test_taskwise_recovery.py，覆盖
|
||||||
完整依赖任务组延后、训练/验证顺序不变、已完成前缀不变、一次回访预算、真实
|
完整依赖任务组延后、训练/验证顺序不变、已完成前缀不变、一次回访预算、真实
|
||||||
coordinator 的带视角错误处理、多视角参考变化不被掩盖、失败后归位且不进入拟合、
|
coordinator 的带视角错误处理、多视角参考变化不被掩盖、失败后归位且不进入拟合、
|
||||||
@@ -583,3 +663,120 @@ Schur 协方差与完整逆矩阵一致、冻结模型拒绝无法解释的阶
|
|||||||
只写入 `calibration_output/software_review_20260922_raw_joint/known_truth/` 和独立软件测试指针；实机 `latest_passed` 未修改。
|
只写入 `calibration_output/software_review_20260922_raw_joint/known_truth/` 和独立软件测试指针；实机 `latest_passed` 未修改。
|
||||||
|
|
||||||
未进行本次新协议全手实测或实机姿态对照；不能把以上软件结果当作现场精度证明。
|
未进行本次新协议全手实测或实机姿态对照；不能把以上软件结果当作现场精度证明。
|
||||||
|
|
||||||
|
|
||||||
|
## O30 轻量原始角点采集与立即补采（2026-09-23）
|
||||||
|
|
||||||
|
新增集中回归 `test_raw_lightweight.py`，不恢复外部删除的历史测试。覆盖当前方向立即补一次、
|
||||||
|
中断预算、仅固定标签在线跟踪、死区/平台、堵转/反向/噪声、静止采样冻结计时、
|
||||||
|
不稳定终点有界退出、采完禁止发送、封存校验、旧协议读取与混合拒绝、旧合成 URDF 重建。
|
||||||
|
|
||||||
|
首组 6 passed、5 failed（4.25 秒）：五个失败是扫描测试没有先进入任务避让姿态，
|
||||||
|
修正夹具起始向量后只复查该组，5 passed（0.98 秒）。后续边界组 10 passed、
|
||||||
|
1 failed（4.18 秒）：竞争控制测试缺少 Start 后反馈，补齐后单项 1 passed（0.77 秒）。
|
||||||
|
完整真实 coordinator 虚拟时钟、单主机位移动标签、108 方向、无补采、归位封存与
|
||||||
|
训练/验证分区检查 1 passed（85.13 秒），属于必要整手调度集成，不是实机通过。
|
||||||
|
旧合成产物 v1/v2 JSON→URDF 重建与终态入口 3 passed（0.87 秒）。
|
||||||
|
删重复图像记录并保留未稳定原始帧后，受影响采集/补采恢复/封存/兼容组
|
||||||
|
7 passed（6.25 秒）。各组存在重叠，不累计为一次全量测试。
|
||||||
|
|
||||||
|
正式 CLI 配置校验通过。实机 `20260923_104837` 去程通过，拇指旋转回程指令 162、
|
||||||
|
反馈 253，累计实际运动无推进 4.005 秒触发保护；未重复启动，控制栈已退出，
|
||||||
|
停在当前位置，未执行自动归位。仅 1/108 方向完成，未封存为完整采集、未发布 URDF。
|
||||||
|
同段移动标签角点最大位移约 0.47 px、固定基准约 0.50 px，未见明显回程运动；
|
||||||
|
不能据此删除停滞保护，也不能宣称全手稳定采集通过。
|
||||||
|
|
||||||
|
补充只读离线入口检查初次失败（3.61 秒）：虚拟相机夹具缺少实际入口要求的
|
||||||
|
`rectified_camera_model` 日志，补齐夹具后在后续组通过，确认缺项拒绝发生于
|
||||||
|
离线验收、原始文件哈希不变且无新运动。
|
||||||
|
用户随后明确授权硬件停滞重试一次：复用现有 StallRetry，O30 原始采集限制为
|
||||||
|
首次尝试加一次重试，等待 3 秒，同方向共享预算；其他硬件保护不进入重试。
|
||||||
|
首组 6 passed、1 failed（4.19 秒）：测试将保护停止发送的最后保持指令也误认为
|
||||||
|
等待期间推进，修正断言后四种实际 coordinator 场景 4 passed（1.31 秒）。
|
||||||
|
运动保护代码和计划身份同步更新，旧策略会话不得混入新断点。
|
||||||
|
|
||||||
|
补充端点阶段切换与反馈抖动回归：180 ms 图像延迟单项通过（7.87 秒），
|
||||||
|
扩大到 300 ms 并增加 4 u8 往返抖动后复现 3 个失败、4 个通过（14.95 秒）。
|
||||||
|
隔离模块验证有限端点等待和推进历史最高值修正，8 passed（7.98 秒），未影响当时运行的实机。
|
||||||
|
用户随后明确端点只看实际发送 SDK 指令：最终采用有效稳态记录中的已发送指令核验端点，
|
||||||
|
不额外增加端点等待、不要求反馈到 0/255，也不把稳态图混入运动覆盖。
|
||||||
|
真实 coordinator 300 ms 延迟、反馈端点仅 10/240、离线误判复核与中断拒绝，
|
||||||
|
加上方向推进、补采、封存及旧协议边界，17 passed（10.04 秒）。
|
||||||
|
SDK 身份错误保留原字段、其他硬件保护和单次停滞重试、旧合成 URDF 重建，
|
||||||
|
9 passed（1.86 秒）。
|
||||||
|
|
||||||
|
实机 `20260923_110651` 完成 35 个方向，原记录 29 个通过、6 个端点误判；
|
||||||
|
在小指 PIP 验证回程因 `o30_sdk_identity_mismatch` 停止，反馈仍约 29.81 Hz。
|
||||||
|
未触发停滞重试，未归位封存，未更新 `latest_passed`。异常身份字段在旧日志中缺失，
|
||||||
|
不能将此状态直接等同于物理通信断开。修正后只读复核同一数据，35 个完整方向均通过，
|
||||||
|
缺 73 个方向，禁止发布，源数据哈希未变化；报告见实现文档。
|
||||||
|
|
||||||
|
最终端点与推进规则下，完整 coordinator 单主机位、108 方向、归位封存及
|
||||||
|
训练/验证分区回归：1 passed（87.51 秒）。该必要整手集成仍仅为软件验证。
|
||||||
|
源码编译和 `git diff --check` 通过；未恢复外部删除的历史测试。
|
||||||
|
|
||||||
|
2026-09-23 用户确认 ID17 在拇指近端指节后修正安装连杆及产品哈希。
|
||||||
|
正式配置加载、拓扑反馈依赖 0/1/6、仅顶机位、17 任务/108 方向校验通过；
|
||||||
|
临时校验脚本首次误用 TagSpec.id，改为 tag_id 后通过，生产加载未失败。
|
||||||
|
既有原始角点、立即补采和旧协议边界回归 3 passed（0.85 秒）。
|
||||||
|
现有真实数据的局部拟合揭示原绑定错误，但修正后最大训练标签误差仍为 8.047 px，
|
||||||
|
原精度门限未通过；软件回归通过不代表实机 URDF 可发布。未重新运动或改写源证据。
|
||||||
|
|
||||||
|
2026-09-23 低精度离线草稿：新增集中 `test_raw_draft.py`，加已有合成正式 URDF
|
||||||
|
重建检查，13 passed（1.55 秒）。覆盖真实零位写入与 FK、缺测关节不改、JSON 零位/
|
||||||
|
曲线/来源篡改拒绝、不可观测零位拒绝、精度超限仍允许草稿、旧 Profile 仅允许标签
|
||||||
|
连杆修正、同步缺失帧不供给范围证据，以及 CLI 不进入运动或正式发布。
|
||||||
|
实测旧数据的正式 CLI 草稿路径已运行，生成完整结构 URDF 并通过标准 ROS 加载；
|
||||||
|
JSON/URDF FK 最大差 4.44e-16，两个拇指零位和四个行程实际改变。
|
||||||
|
独立精度验收仍失败，输出 `accuracy_passed=false`，没有更新 `latest_passed`。
|
||||||
|
第一次实数据导出因原始帧含空指令失败，修复共享证据过滤后 v2 成功；未删除失败审计。
|
||||||
|
|
||||||
|
2026-09-23 全手 15 零位/20 行程目标及采集后自动导出：受影响范围与真实 runner 生命周期
|
||||||
|
5 passed（1.75 秒），验证完整目标、部分数据不能冒充全手，以及成功/缺项/求解失败三种
|
||||||
|
情况下 SDK 栈、监控节点和自建 ROS 上下文均先关闭，离线阶段从不重新启动运动。
|
||||||
|
补充 Profile 原字节快照后，仅复查三个生命周期场景，3 passed（1.66 秒）。
|
||||||
|
正式 CLI --validate-only 通过，明确列出 15 个零位和 20 个行程目标。
|
||||||
|
|
||||||
|
必要的一次全手数学检查：独立 XML FK 生成 2028 帧训练角点，463 参数图像模型单初值
|
||||||
|
12 次联合迭代收敛；包括生成数据和秩检查共 31.82 秒，最大零位差 0.001054°。
|
||||||
|
复用保存参数进行完整 JSON→URDF 导出与标准加载，不重复拟合；15 个零位修正、五个
|
||||||
|
末端零位保留、20 个行程、最终 FK 最大差 5.55e-16 均通过。非实机验收、不更新正式指针。
|
||||||
|
|
||||||
|
2026-09-23 ID10 调整后的草稿断点：新增 `test_raw_remount.py`，验证 78 个连续通过方向
|
||||||
|
保留、旧 ID10 角点排除、原始像素不改、已用补采次数保留、重复调整声明拒绝、未知/
|
||||||
|
固定标签拒绝，以及固定基准和已知标签漂移仍拒绝。基准不可见标签只能进入草稿，不能
|
||||||
|
正式发布。首轮 5 passed，测试夹具共享列表导致 1 failed；修正夹具后，对失败项、CLI、
|
||||||
|
中断预算及受影响草稿契约运行 19 passed（2.74 秒）。没有恢复已删除的历史测试。
|
||||||
|
随后按 CAD 拓扑隔离整条受影响手指，验证旧观测不混用、已开始扫描禁止扩大排除范围，
|
||||||
|
受影响 6 passed（1.14 秒）。
|
||||||
|
|
||||||
|
2026-09-23 中指侧摆 0–80 短段：按已声明段长度缩放反馈分箱密度，保留图像/指令覆盖、
|
||||||
|
实际运动分辨率、4 秒推进及其他硬件保护。相关短段、堵转/反向/噪声、硬件保护、离线
|
||||||
|
生命周期与范围证据检查 14 passed（3.02 秒）。只读复核 `20260923_125329` 的两个
|
||||||
|
第二次完整尝试均通过，原始失败记录未改写，结果保存为 `short_segment_reassessment.json`。
|
||||||
|
## 离线零位方向回归
|
||||||
|
|
||||||
|
`test_raw_draft.py` 中的 `parallel_root_motion` 用同一批不变角点核对平行根轴方向，
|
||||||
|
四指 SDK/URDF 符号反转时必须拒绝，不能通过关节零位吸收错误。
|
||||||
|
`old_capture_model_correction` 验证离线绑定/方向修订仍保留原始采集身份，并拒绝速度变更。
|
||||||
|
方向用例来自首轮实采角点节选 `fixtures/o30_recorded_root_directions.json.gz`，
|
||||||
|
保留来源 SHA256，不含验证轮。其余数值回归仍使用合成数据；不发送硬件指令。
|
||||||
|
|
||||||
|
2026-09-23 拇指独立原始采集：局部范围编译保留 `raw_joint_2_plus_1`，清除被保留关节的
|
||||||
|
零位测量声明；训练策略允许经过范围契约验证的 O30 局部配置。复用原执行器和补采预算，
|
||||||
|
未修改拟合模型或运动保护。新增范围往返序列化、24 单元原路线/分区一致、立即补一次与归位后
|
||||||
|
结束测试，连同全手原调度两项回归共 4 passed（1.03 秒）。
|
||||||
|
实机会话 `thumb_recapture_20260923_175829/O30_RIGHT_001/20260923_180129` 在启动归位阶段
|
||||||
|
MCP 指令 0/反馈 120，两次无推进后停止；正式扫描 0/24，不能记为实机采集通过。
|
||||||
|
标定控制链已关闭，原全手数据未修改，等待现场机械接触观察。
|
||||||
|
|
||||||
|
2026-09-23 新外参拇指与旧四指离线组合：修复独立验证总判定的 NumPy bool_ JSON
|
||||||
|
序列化失败；新增冻结旧参考参数的图像拟合更新与显式局部验证范围。参数按语义转移，
|
||||||
|
不共享新旧相机外参，不改变 CAD 轴，无观测增量冻结，零位存在自由规范时拒绝。
|
||||||
|
`test_raw_joint_update.py test_raw_draft.py` 共 37 passed（20.74 秒）；随后加入零位
|
||||||
|
可观测性保护，更新工具相关 3 passed（0.19 秒）。全手默认验证仍拒绝缺失关节，
|
||||||
|
局部报告不得视为全手验收。未恢复删除的历史测试。
|
||||||
|
实采组合草稿通过 JSON 重建、最终 FK（7.36e-16）、标准加载与 MuJoCo 编译。
|
||||||
|
曲线模型角度检查通过不等于 URDF 线性驱动通过：纯 URDF yaw 最大误差约 3.52°，
|
||||||
|
精度未认证，不更新 latest_passed。详见
|
||||||
|
`calibration_output/thumb_recapture_20260923_175829/offline_combined_new_thumb_draft/README.md`。
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
schema_version: 3
|
schema_version: 3
|
||||||
profile_id: O30/right/o30_right_18/v1
|
profile_id: O30/right/o30_right_18/v1
|
||||||
profile_config: package://linkerhand_calibration/config/profiles/o30_right_18.yaml
|
profile_config: package://linkerhand_calibration/config/profiles/o30_right_18.yaml
|
||||||
profile_config_sha256: d782fcea36ba753af18ebde4a48c9f4b74d0a93fd27359a8fbfd70b9d23a49c7
|
profile_config_sha256: 0de1efee3ca95a52d04f289f4a118d2119b81e4a84e247f56f2176ea64130b39
|
||||||
model: O30
|
model: O30
|
||||||
side: right
|
side: right
|
||||||
tag_layout: o30_right_18
|
tag_layout: o30_right_18
|
||||||
@@ -26,8 +26,8 @@ cameras:
|
|||||||
artifacts:
|
artifacts:
|
||||||
source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf
|
source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf
|
||||||
source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b
|
source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b
|
||||||
camera_extrinsics: config/o30_three_camera_extrinsics_20260922_101452.yaml
|
camera_extrinsics: config/o30_three_camera_extrinsics_20260923_181333.yaml
|
||||||
camera_extrinsics_sha256: d28d3e8771f5bc20d4ca6c9bfd11977f26fefb8478d125a113c013fb7a208120
|
camera_extrinsics_sha256: e65a09b31bf10aed2fe0926585d446d209929fe7fc84f55f641b0b08a9ef41aa
|
||||||
calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml
|
calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml
|
||||||
calibration_config_sha256: aeb7826ac00e7ab4c32b131319b34bc11a849c668f268eec2bb4fa12fb081970
|
calibration_config_sha256: aeb7826ac00e7ab4c32b131319b34bc11a849c668f268eec2bb4fa12fb081970
|
||||||
tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml
|
tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml
|
||||||
|
|||||||
@@ -70,7 +70,7 @@ command:
|
|||||||
'17': 17
|
'17': 17
|
||||||
'18': 18
|
'18': 18
|
||||||
'19': 19
|
'19': 19
|
||||||
sdk_to_joint_direction: [1, 1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
|
sdk_to_joint_direction: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
|
||||||
vision:
|
vision:
|
||||||
views:
|
views:
|
||||||
- name: front
|
- name: front
|
||||||
@@ -94,22 +94,22 @@ vision:
|
|||||||
id: 12
|
id: 12
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
size_m: 0.016
|
size_m: 0.016
|
||||||
link: pinky_metacarpals
|
link: pinky_middle
|
||||||
- role: ring_mcp_roll
|
- role: ring_mcp_roll
|
||||||
id: 13
|
id: 13
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
size_m: 0.016
|
size_m: 0.016
|
||||||
link: ring_metacarpals
|
link: ring_middle
|
||||||
- role: middle_mcp_roll
|
- role: middle_mcp_roll
|
||||||
id: 14
|
id: 14
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
size_m: 0.016
|
size_m: 0.016
|
||||||
link: middle_metacarpals
|
link: middle_middle
|
||||||
- role: index_mcp_roll
|
- role: index_mcp_roll
|
||||||
id: 15
|
id: 15
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
size_m: 0.016
|
size_m: 0.016
|
||||||
link: index_metacarpals
|
link: index_middle
|
||||||
- name: side
|
- name: side
|
||||||
tags:
|
tags:
|
||||||
- role: side_base
|
- role: side_base
|
||||||
@@ -168,7 +168,8 @@ vision:
|
|||||||
id: 17
|
id: 17
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
size_m: 0.016
|
size_m: 0.016
|
||||||
link: thumb_metacarpals_base2
|
# 顶机位标签实际贴在近端指节，随 CMC 和 MCP 运动，不随 IP 运动。
|
||||||
|
link: thumb_proximal
|
||||||
common_frame: calibration_common
|
common_frame: calibration_common
|
||||||
extrinsic_reference_view: front
|
extrinsic_reference_view: front
|
||||||
extrinsics_quality_limits:
|
extrinsics_quality_limits:
|
||||||
|
|||||||
@@ -16,6 +16,8 @@ STAGED = "staged_2_to_3"
|
|||||||
TASKWISE_STAGED = "taskwise_staged_2_to_3"
|
TASKWISE_STAGED = "taskwise_staged_2_to_3"
|
||||||
TASKWISE_TWO_ROUND = "taskwise_2_plus_1"
|
TASKWISE_TWO_ROUND = "taskwise_2_plus_1"
|
||||||
RAW_JOINT = "raw_joint_2_plus_1"
|
RAW_JOINT = "raw_joint_2_plus_1"
|
||||||
|
RAW_PROTOCOL = "raw_joint_images_v2"
|
||||||
|
LEGACY_RAW_PROTOCOL = "raw_joint_images_v1"
|
||||||
STAGED_POLICIES = frozenset((STAGED, TASKWISE_STAGED))
|
STAGED_POLICIES = frozenset((STAGED, TASKWISE_STAGED))
|
||||||
PREPARATION_POLICIES = STAGED_POLICIES | {TASKWISE_TWO_ROUND}
|
PREPARATION_POLICIES = STAGED_POLICIES | {TASKWISE_TWO_ROUND}
|
||||||
LEGACY_TRAINING = (0, 1, 2)
|
LEGACY_TRAINING = (0, 1, 2)
|
||||||
@@ -75,8 +77,8 @@ class CapturePlan:
|
|||||||
def task(self, key):
|
def task(self, key):
|
||||||
return next(task for task in self.tasks if task.task_key == key)
|
return next(task for task in self.tasks if task.task_key == key)
|
||||||
|
|
||||||
def as_dict(self):
|
def as_dict(self, *, legacy_raw=False):
|
||||||
version = ("capture_plan_v6_raw_joint" if self.policy == RAW_JOINT else
|
version = (("capture_plan_v6_raw_joint" if legacy_raw else "capture_plan_v7_raw_joint") if self.policy == RAW_JOINT else
|
||||||
"capture_plan_v5" if self.policy == TASKWISE_TWO_ROUND else
|
"capture_plan_v5" if self.policy == TASKWISE_TWO_ROUND else
|
||||||
"capture_plan_v3" if self.policy == TASKWISE_STAGED else
|
"capture_plan_v3" if self.policy == TASKWISE_STAGED else
|
||||||
"capture_plan_v2" if self.policy == STAGED else PLAN_VERSION)
|
"capture_plan_v2" if self.policy == STAGED else PLAN_VERSION)
|
||||||
@@ -108,12 +110,15 @@ class CapturePlan:
|
|||||||
if self.policy == RAW_JOINT:
|
if self.policy == RAW_JOINT:
|
||||||
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
|
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
|
||||||
result["observation_protocol"] = {
|
result["observation_protocol"] = {
|
||||||
"version": "raw_joint_images_v1",
|
"version": LEGACY_RAW_PROTOCOL if legacy_raw else RAW_PROTOCOL,
|
||||||
"online_geometry_authorization": False,
|
"online_geometry_authorization": False,
|
||||||
"maximum_unit_recaptures": 1,
|
"maximum_unit_recaptures": 1,
|
||||||
"recapture_order": "queue_tail",
|
"recapture_order": "queue_tail" if legacy_raw else "immediate",
|
||||||
"incomplete_capture_blocks_publication": True,
|
"incomplete_capture_blocks_publication": True,
|
||||||
}
|
}
|
||||||
|
if not legacy_raw:
|
||||||
|
result["observation_protocol"].update(hardware_stall_retries=1,
|
||||||
|
stall_retry_delay_seconds=3.0, motion_progress_scope="direction_active_time")
|
||||||
return result
|
return result
|
||||||
|
|
||||||
@property
|
@property
|
||||||
@@ -168,8 +173,8 @@ def select_task_training(profile, task_key, cycles):
|
|||||||
def configure_training(profile, policy):
|
def configure_training(profile, policy):
|
||||||
if policy not in {FIXED, ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT}:
|
if policy not in {FIXED, ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT}:
|
||||||
raise ValueError("unknown_training_policy")
|
raise ValueError("unknown_training_policy")
|
||||||
if policy == RAW_JOINT and (profile.key.profile_id != 'O30/right/o30_right_18/v1' or profile.retained_joints):
|
if policy == RAW_JOINT and profile.key.profile_id != 'O30/right/o30_right_18/v1':
|
||||||
raise ValueError("raw_joint_capture_requires_o30_full_hand_profile")
|
raise ValueError("raw_joint_capture_requires_o30_profile")
|
||||||
if profile.artifacts.output_schema_version in {2, 3} and (
|
if profile.artifacts.output_schema_version in {2, 3} and (
|
||||||
tuple(profile.quality.training_cycles) != LEGACY_TRAINING
|
tuple(profile.quality.training_cycles) != LEGACY_TRAINING
|
||||||
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
|
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
|
||||||
|
|||||||
@@ -17,6 +17,8 @@ class TaskStatus:
|
|||||||
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
|
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
|
||||||
default_factory=dict
|
default_factory=dict
|
||||||
)
|
)
|
||||||
|
segment_key: str = ""
|
||||||
|
segment_commands: Mapping[str, tuple[float, float]] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
@@ -0,0 +1,77 @@
|
|||||||
|
"""Training-only support for the physically shared turnaround endpoints.
|
||||||
|
|
||||||
|
The last incoming command owns the turnaround image. An unobserved outgoing
|
||||||
|
endpoint is a continuity constraint, never a second independent measurement.
|
||||||
|
Interior parameters and endpoints with actual directional evidence stay free.
|
||||||
|
"""
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
TURNAROUND_POLICY = 'observed_turnaround_continuity_v1'
|
||||||
|
|
||||||
|
|
||||||
|
def compile_turnaround_constraints(bundle):
|
||||||
|
constraints = {}
|
||||||
|
for name, columns in bundle.curve_columns.items():
|
||||||
|
channel = bundle.channels[name]
|
||||||
|
indices = sorted({int(index) for role, data in bundle.observations.items()
|
||||||
|
if any(joint.name == bundle.cad_names[name] for joint in bundle.chains[role])
|
||||||
|
for index in data[0]})
|
||||||
|
if not indices:
|
||||||
|
continue
|
||||||
|
commands = bundle.commands[indices, channel]
|
||||||
|
directions = bundle.directions[indices, channel]
|
||||||
|
for node in (0, len(bundle.knots[name])-1):
|
||||||
|
for recipient in (0, 1):
|
||||||
|
column = int(columns[recipient, node])
|
||||||
|
# Unknown startup history observes the common endpoint; it
|
||||||
|
# does not independently identify the outgoing branch.
|
||||||
|
at_endpoint = commands == bundle.knots[name][node]
|
||||||
|
if column < 0 or np.any(at_endpoint & (directions == recipient)):
|
||||||
|
continue
|
||||||
|
donor = 1-recipient
|
||||||
|
sources = [bundle.rows[index] for index in indices
|
||||||
|
if bundle.commands[index, channel] == bundle.knots[name][node]
|
||||||
|
and bundle.directions[index, channel] == donor]
|
||||||
|
if not sources:
|
||||||
|
continue
|
||||||
|
constraints.setdefault(name, []).append(dict(node=node, recipient=recipient,
|
||||||
|
donor=donor, column=column, command=float(bundle.knots[name][node]),
|
||||||
|
source_sample_ids=sorted({f"{row['view']}:{row['image_stamp_ns']}" for row in sources}),
|
||||||
|
source_cycles=sorted({row['cycle'] for row in sources})))
|
||||||
|
return constraints
|
||||||
|
|
||||||
|
|
||||||
|
def validate_turnaround_mapping(mapping, support):
|
||||||
|
"""Read-back must preserve the saved constraint, including its sources."""
|
||||||
|
if support is None: # Historical artifacts retain their old contract.
|
||||||
|
return
|
||||||
|
if support.get('policy') != TURNAROUND_POLICY or not isinstance(support.get('constraints'), list):
|
||||||
|
raise ValueError('invalid_turnaround_support:' + mapping.joint)
|
||||||
|
endpoints = support.get('endpoints', ())
|
||||||
|
if (len(endpoints) != 2 or [row.get('command') for row in endpoints]
|
||||||
|
!= [mapping.knots[0], mapping.knots[-1]]):
|
||||||
|
raise ValueError('invalid_turnaround_endpoints:' + mapping.joint)
|
||||||
|
for endpoint in endpoints:
|
||||||
|
if not endpoint.get('samples') or any(
|
||||||
|
sample.get('cycle') not in (0, 1) or not isinstance(sample.get('id'), str)
|
||||||
|
or not sample['id'] or sample.get('direction') not in ('', 'increasing', 'decreasing')
|
||||||
|
for sample in endpoint['samples']):
|
||||||
|
raise ValueError('invalid_turnaround_samples:' + mapping.joint)
|
||||||
|
branches = (mapping.increasing_rad, mapping.decreasing_rad)
|
||||||
|
seen = set()
|
||||||
|
for row in support['constraints']:
|
||||||
|
node, recipient, donor = row['node'], row['recipient'], row['donor']
|
||||||
|
if (node not in (0, len(mapping.knots)-1) or recipient not in (0, 1)
|
||||||
|
or donor != 1-recipient or (node, recipient) in seen
|
||||||
|
or row['command'] != mapping.knots[node]
|
||||||
|
or not row['source_sample_ids'] or not row['source_cycles']
|
||||||
|
or not set(row['source_cycles']) <= {0, 1}
|
||||||
|
or abs(branches[recipient][node]-branches[donor][node]) > 1e-10):
|
||||||
|
raise ValueError('turnaround_mapping_differs_from_evidence:' + mapping.joint)
|
||||||
|
seen.add((node, recipient))
|
||||||
|
endpoint = endpoints[0 if node == 0 else 1]
|
||||||
|
observed = {sample['id'] for sample in endpoint['samples']
|
||||||
|
if sample['direction'] == ('increasing', 'decreasing')[donor]}
|
||||||
|
if not observed.intersection(row['source_sample_ids']):
|
||||||
|
raise ValueError('turnaround_source_not_observed:' + mapping.joint)
|
||||||
@@ -0,0 +1,322 @@
|
|||||||
|
"""Approximate command curves using the existing CAD image objective.
|
||||||
|
|
||||||
|
Drafts estimate supported internal zeros. Unobserved root frames and declared
|
||||||
|
CAD leaf zeros remain explicit premises, never arbitrary measured offsets.
|
||||||
|
"""
|
||||||
|
from collections import defaultdict
|
||||||
|
from dataclasses import replace
|
||||||
|
from itertools import product
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
from ..domain.capture_plan import task_for_joint
|
||||||
|
from ..geometry.tag_pose.motion_evidence import _axis
|
||||||
|
from ..geometry.tag_pose.urdf_command_images import UrdfCommandImages, pose_vector
|
||||||
|
from .motion_fit import joint_input_direction
|
||||||
|
|
||||||
|
UPRIGHT_SPLAY_JOINTS = frozenset(finger+'_mcp_roll' for finger in ('index', 'middle', 'ring', 'pinky'))
|
||||||
|
UPRIGHT_SPLAY_REFERENCE = 'operator_defined_upright_splay_baseline'
|
||||||
|
|
||||||
|
|
||||||
|
def define_upright_splay_zero(profile, model):
|
||||||
|
"""Use the operator's upright baseline as a definition, not a measurement."""
|
||||||
|
if (profile.key.model != 'O30' or profile.key.side != 'right'
|
||||||
|
or not UPRIGHT_SPLAY_JOINTS <= profile.zero.active_joints):
|
||||||
|
raise ValueError('draft_upright_splay_requires_o30_right_four_fingers')
|
||||||
|
for name in UPRIGHT_SPLAY_JOINTS:
|
||||||
|
finger = name.split('_')[0]
|
||||||
|
positions = [model.link_transform(profile.command.urdf_joint_by_joint.get(joint, joint),
|
||||||
|
zero_offsets={}, joint_angles={}, independent_mimic_angles=True)[:3, 3]
|
||||||
|
for joint in (finger+'_mcp_pitch', finger+'_pip')]
|
||||||
|
direction = positions[1]-positions[0]
|
||||||
|
if not np.allclose(direction/np.linalg.norm(direction), [0., 0., 1.], atol=1e-6):
|
||||||
|
raise ValueError('draft_source_zero_is_not_upright:' + name)
|
||||||
|
assumptions = dict(profile.zero.cad_zero_assumptions)
|
||||||
|
assumptions.update({name: UPRIGHT_SPLAY_REFERENCE for name in UPRIGHT_SPLAY_JOINTS})
|
||||||
|
return replace(profile, zero=replace(profile.zero, cad_zero_assumptions=assumptions))
|
||||||
|
|
||||||
|
|
||||||
|
def draft_training_coverage(completed_units, units):
|
||||||
|
"""Audit accepted directions without borrowing from independent validation."""
|
||||||
|
completed = set(completed_units)
|
||||||
|
coverage = {}
|
||||||
|
for task in sorted({unit.task_key for unit in units}):
|
||||||
|
cycles = {}
|
||||||
|
for cycle in (0, 1):
|
||||||
|
expected = {unit.identity for unit in units if unit.task_key == task and unit.cycle == cycle}
|
||||||
|
accepted = expected & completed
|
||||||
|
cycles[cycle] = dict(expected_units=len(expected), accepted_units=len(accepted),
|
||||||
|
complete=bool(expected) and accepted == expected)
|
||||||
|
complete = [cycle for cycle, row in cycles.items() if row['complete']]
|
||||||
|
coverage[task] = dict(cycles=cycles, complete_cycles=complete,
|
||||||
|
initialization_cycle=complete[0] if complete else None)
|
||||||
|
return coverage
|
||||||
|
|
||||||
|
|
||||||
|
def draft_targets(profile, selected=None):
|
||||||
|
"""Report the requested whole-hand result independently of partial output."""
|
||||||
|
target_ranges = set(profile.zero.active_joints)
|
||||||
|
retained_leaves = set(profile.zero.cad_zero_assumptions)
|
||||||
|
target_zeros = target_ranges-retained_leaves
|
||||||
|
measured_ranges = set() if selected is None else set(selected.zero.active_joints)
|
||||||
|
measured_zeros = set() if selected is None else measured_ranges-set(selected.zero.cad_zero_assumptions)
|
||||||
|
defined_zeros = set() if selected is None else {name for name, reason in
|
||||||
|
selected.zero.cad_zero_assumptions.items() if reason == UPRIGHT_SPLAY_REFERENCE}
|
||||||
|
return dict(zero_target_joints=sorted(target_zeros), range_target_joints=sorted(target_ranges),
|
||||||
|
retained_leaf_zero_joints=sorted(retained_leaves), estimated_zero_joints=sorted(measured_zeros),
|
||||||
|
defined_zero_joints=sorted(defined_zeros),
|
||||||
|
estimated_range_joints=sorted(measured_ranges),
|
||||||
|
missing_zero_joints=sorted(target_zeros-measured_zeros-defined_zeros),
|
||||||
|
missing_range_joints=sorted(target_ranges-measured_ranges),
|
||||||
|
full_target_covered=(target_zeros <= measured_zeros | defined_zeros and target_ranges <= measured_ranges))
|
||||||
|
|
||||||
|
|
||||||
|
def draft_profile(profile, model, completed_units, units, training):
|
||||||
|
"""One complete training cycle supports a draft, with observed ancestors."""
|
||||||
|
coverage = draft_training_coverage(completed_units, units)
|
||||||
|
tasks = {task for task, row in coverage.items() if row['complete_cycles']}
|
||||||
|
names = {name for task in profile.motion.tasks if task.key in tasks for name in task.joints}
|
||||||
|
cad_names = {name: profile.command.urdf_joint_by_joint.get(name, name) for name in names}
|
||||||
|
observed = set(cad_names.values())
|
||||||
|
names = frozenset(name for name in names if all(j.kind == 'fixed' or j.name in observed
|
||||||
|
for j in model._chain(cad_names[name])))
|
||||||
|
if not names:
|
||||||
|
raise ValueError('draft_no_complete_ancestor_supported_training')
|
||||||
|
from ...profiles.observations import compile_tag_links
|
||||||
|
links = compile_tag_links(profile, model)
|
||||||
|
observed = {cad_names[name] for name in names}
|
||||||
|
visible = {role for row in training for role in row.get('tags', {})}
|
||||||
|
roles = {role for role, link in links.items() if role in visible and (link == model.root_link or all(
|
||||||
|
j.kind == 'fixed' or j.name in observed
|
||||||
|
for j in model._chain(model.parent_joint_by_child[link])))}
|
||||||
|
# Fixed tags belong to the root, which has no parent joint.
|
||||||
|
views = tuple(replace(view, tags=tuple(tag for tag in view.tags
|
||||||
|
if tag.fixed_reference or tag.role in roles)) for view in profile.vision.views)
|
||||||
|
assumptions = {name: reason for name, reason in profile.zero.cad_zero_assumptions.items() if name in names}
|
||||||
|
for name, observer in profile.zero.spatial.get('offset_observer_joint', {}).items():
|
||||||
|
if name in names and observer not in names:
|
||||||
|
assumptions[name] = 'partial_missing_zero_observer:' + observer
|
||||||
|
roots = [name for name in names if not any(j.kind != 'fixed'
|
||||||
|
for j in model._chain(cad_names[name])[:-1])]
|
||||||
|
# With only one root chain, rotating palm and root zero together leaves
|
||||||
|
# every image identical. Fix that gauge, while fitting the internal zeros.
|
||||||
|
if len(roots) == 1:
|
||||||
|
assumptions[roots[0]] = 'partial_single_root_zero_unobservable_without_palm_reference'
|
||||||
|
selected = replace(profile,
|
||||||
|
zero=replace(profile.zero, active_joints=names,
|
||||||
|
cad_zero_assumptions=assumptions),
|
||||||
|
motion=replace(profile.motion, tasks=tuple(task for task in profile.motion.tasks
|
||||||
|
if task.key in tasks and set(task.joints) <= names)),
|
||||||
|
vision=replace(profile.vision, views=views))
|
||||||
|
required = {profile.measurement.measurements[name].child_role for name in names}
|
||||||
|
if not required <= roles:
|
||||||
|
raise ValueError('draft_required_training_tags_missing:' + str(sorted(required-roles)))
|
||||||
|
return selected
|
||||||
|
|
||||||
|
|
||||||
|
class DraftCommandImages(UrdfCommandImages):
|
||||||
|
"""Shared image objective, with exact repeated-pose least-squares reduction."""
|
||||||
|
|
||||||
|
# Numerical stopping for an explicitly uncertified draft; release angular,
|
||||||
|
# image, observability and final-file acceptance limits remain independent.
|
||||||
|
solver_ftol = 1e-5
|
||||||
|
|
||||||
|
def __init__(self, profile, model, extrinsics, rows, tag_links, *, initialization_cycles=None,
|
||||||
|
training_initialization=None):
|
||||||
|
super().__init__(profile, model, extrinsics, rows, tag_links)
|
||||||
|
self.training_initialization = training_initialization
|
||||||
|
self.initialization_cycles = (dict(initialization_cycles) if initialization_cycles is not None
|
||||||
|
else {task.key: 0 for task in profile.motion.tasks})
|
||||||
|
if any(self.initialization_cycles.get(task.key) not in (0, 1) for task in profile.motion.tasks):
|
||||||
|
raise ValueError('draft_initialization_training_cycle_required')
|
||||||
|
self.original_observations = self.observations
|
||||||
|
self.observations, weights, within_group_error = {}, [], 0.
|
||||||
|
local_names = {cad: name for name, cad in self.cad_names.items()}
|
||||||
|
for role, (indices, pixels, matrices, objects) in self.original_observations.items():
|
||||||
|
channels = [self.channels[local_names[joint.name]] for joint in self.chains[role]
|
||||||
|
if joint.kind != 'fixed']
|
||||||
|
groups = defaultdict(list)
|
||||||
|
for position, index in enumerate(indices):
|
||||||
|
key = (tuple(self.commands[index, channels]), tuple(self.directions[index, channels]),
|
||||||
|
matrices[position].tobytes())
|
||||||
|
groups[key].append(position)
|
||||||
|
representatives, means = [], []
|
||||||
|
for positions in groups.values():
|
||||||
|
mean = pixels[positions].mean(axis=0)
|
||||||
|
within_group_error += float(np.sum((pixels[positions]-mean)**2))
|
||||||
|
representatives.append(positions[0])
|
||||||
|
means.append(mean)
|
||||||
|
weights.extend([np.sqrt(len(positions))]*8)
|
||||||
|
self.observations[role] = (indices[representatives], np.asarray(means),
|
||||||
|
matrices[representatives], objects)
|
||||||
|
self.residual_weights = np.asarray(weights)
|
||||||
|
self.within_group_error = within_group_error
|
||||||
|
self.original_residual_count = sum(len(data[0])*8 for data in self.original_observations.values())
|
||||||
|
|
||||||
|
def residual(self, parameters):
|
||||||
|
# sum ||prediction - pixel_i||² = n ||prediction - mean||² + scatter.
|
||||||
|
# Keep scatter in the objective, including the solver stopping test.
|
||||||
|
return np.r_[super().residual(parameters)*self.residual_weights,
|
||||||
|
np.sqrt(self.within_group_error), np.zeros(7)]
|
||||||
|
|
||||||
|
def jacobian(self, parameters):
|
||||||
|
weighted = super().jacobian(parameters)*self.residual_weights[:, None]
|
||||||
|
return np.vstack((weighted, np.zeros((8, len(parameters)))))
|
||||||
|
|
||||||
|
def original_residual(self, parameters):
|
||||||
|
predicted = self.project_observations(parameters, self.commands, self.directions, self.original_observations)
|
||||||
|
return np.concatenate([(predicted[role]-data[1]).ravel()
|
||||||
|
for role, data in self.original_observations.items()])
|
||||||
|
|
||||||
|
def audit_root_directions(self):
|
||||||
|
"""Reject opposite SDK signs for CAD-parallel roots, before fitting zeros.
|
||||||
|
|
||||||
|
Only complete training cycles enter this check. Both planar candidates
|
||||||
|
and both scan directions are retained; an ambiguous axis is not a
|
||||||
|
direction correction. Nothing here changes commands or chooses a pose.
|
||||||
|
"""
|
||||||
|
roots = [name for name in self.profile.zero.spatial.get('root_anchor_joints', ())
|
||||||
|
if name in self.joints]
|
||||||
|
axes = {}
|
||||||
|
for name in roots:
|
||||||
|
spec = self.profile.measurement.measurements[name]
|
||||||
|
task = task_for_joint(self.profile, name)
|
||||||
|
channel = self.channels[name]
|
||||||
|
observed = []
|
||||||
|
for direction in range(2):
|
||||||
|
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
|
||||||
|
if len(indices) < 3:
|
||||||
|
continue
|
||||||
|
for branch in (0, 1):
|
||||||
|
_, poses = self.motion_poses(indices, spec.child_role, branch)
|
||||||
|
axis, phase = _axis(Rotation.from_matrix(np.asarray(poses)[:, :3, :3]))
|
||||||
|
if abs(phase[-1]-phase[0]) < np.radians(15):
|
||||||
|
continue
|
||||||
|
observed.append(axis*np.sign(phase[-1]-phase[0])
|
||||||
|
*joint_input_direction(self.profile, self.model, name))
|
||||||
|
axes[name] = observed
|
||||||
|
comparisons = []
|
||||||
|
for i, first in enumerate(roots):
|
||||||
|
a, _ = self.model.axis_line(self.cad_names[first], zero_offsets={}, joint_angles={})
|
||||||
|
for second in roots[i+1:]:
|
||||||
|
b, _ = self.model.axis_line(self.cad_names[second], zero_offsets={}, joint_angles={})
|
||||||
|
if abs(a @ b) < .999 or not axes[first] or not axes[second]:
|
||||||
|
continue
|
||||||
|
dots = np.asarray(axes[first]) @ np.asarray(axes[second]).T * np.sign(a @ b)
|
||||||
|
comparisons.append(dict(joints=[first, second], minimum_cosine=float(dots.min()),
|
||||||
|
maximum_cosine=float(dots.max()), reversed_for_all_candidates=bool(dots.max() < -.5)))
|
||||||
|
return dict(training_only=True, validation_used=False, comparisons=comparisons,
|
||||||
|
passed=not any(row['reversed_for_all_candidates'] for row in comparisons))
|
||||||
|
|
||||||
|
def initialize(self, branches):
|
||||||
|
roles = sorted(self.tags)
|
||||||
|
if len(branches) != min(6, len(roles)) or any(b not in (0, 1) for b in branches):
|
||||||
|
raise ValueError('draft_initialization_seed_invalid')
|
||||||
|
branch_by_role = {role: branches[min(i, len(branches)-1)] for i, role in enumerate(roles)}
|
||||||
|
initial = np.zeros(len(self.scale))
|
||||||
|
lines = {}
|
||||||
|
for name in self.joints:
|
||||||
|
task = task_for_joint(self.profile, name)
|
||||||
|
spec = self.profile.measurement.measurements[name]
|
||||||
|
channel = self.channels[name]
|
||||||
|
sign = joint_input_direction(self.profile, self.model, name)
|
||||||
|
for direction in range(2):
|
||||||
|
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
|
||||||
|
if len(indices) < 3:
|
||||||
|
raise ValueError('draft_directional_training_missing:' + name)
|
||||||
|
indices, poses = self.motion_poses(indices, spec.child_role, branch_by_role[spec.child_role])
|
||||||
|
axis, phase = _axis(Rotation.from_matrix(poses[:, :3, :3]))
|
||||||
|
if (phase[-1]-phase[0])*sign < 0:
|
||||||
|
axis, phase = -axis, -phase
|
||||||
|
commands = self.commands[indices, channel]
|
||||||
|
phase -= np.interp(self.profile.command.baseline_values[channel], commands, phase)
|
||||||
|
self.set_initial_curve(initial, name, direction, np.interp(self.knots[name], commands, phase))
|
||||||
|
if direction == 0:
|
||||||
|
turns = poses[:, :3, :3] @ poses[0, :3, :3].T
|
||||||
|
blocks = np.eye(3)-turns
|
||||||
|
target = poses[:, :3, 3]-np.einsum('nij,j->ni', turns, poses[0, :3, 3])
|
||||||
|
lines[name] = (axis, np.linalg.lstsq(blocks.reshape(-1, 3), target.ravel(), rcond=1e-6)[0], indices[0])
|
||||||
|
angles = self.joint_angles(initial, self.commands, self.directions)
|
||||||
|
cad_axes, axes, cad_points, points = [], [], [], []
|
||||||
|
for name, (axis, point, index) in lines.items():
|
||||||
|
cad_axis, cad_point = self.model.axis_line(self.cad_names[name], zero_offsets={},
|
||||||
|
joint_angles={joint: values[index] for joint, values in angles.items()}, independent_mimic_angles=True)
|
||||||
|
cad_axes.append(cad_axis); axes.append(axis)
|
||||||
|
cad_points.append(cad_point); points.append(point)
|
||||||
|
if np.linalg.matrix_rank(np.asarray(cad_axes), tol=.01) < 2:
|
||||||
|
raise ValueError('draft_initialization_requires_nonparallel_observed_axes')
|
||||||
|
rotation, _ = Rotation.align_vectors(np.asarray(axes), np.asarray(cad_axes))
|
||||||
|
blocks = np.asarray([np.eye(3)-np.outer(axis, axis) for axis in axes])
|
||||||
|
targets = np.einsum('nij,nj->ni', blocks, np.asarray(points)-rotation.apply(cad_points))
|
||||||
|
initial[:3] = rotation.as_rotvec()
|
||||||
|
initial[3:6] = np.linalg.lstsq(blocks.reshape(-1, 3), targets.ravel(), rcond=1e-6)[0]
|
||||||
|
if self.training_initialization is not None:
|
||||||
|
hint = self.training_initialization
|
||||||
|
initial[:6] = hint['palm']
|
||||||
|
for name, mapping in hint['mappings'].items():
|
||||||
|
if name not in self.joints:
|
||||||
|
continue
|
||||||
|
if name in self.zero_columns:
|
||||||
|
initial[self.zero_columns[name]] = hint['offsets'][name]
|
||||||
|
for direction, values in enumerate((mapping.increasing_rad, mapping.decreasing_rad)):
|
||||||
|
self.set_initial_curve(initial, name, direction,
|
||||||
|
np.interp(self.knots[name], mapping.knots, values))
|
||||||
|
transforms = self.link_poses(initial, self.commands, self.directions)
|
||||||
|
for role, column in self.mount_columns.items():
|
||||||
|
index = self.observations[role][0][0]
|
||||||
|
initial[column:column+6] = pose_vector(np.linalg.inv(transforms[self.tag_links[role]][index])
|
||||||
|
@ self._pose(index, role, branch_by_role[role]))
|
||||||
|
self._initialization_costs[tuple(branches)] = self._refine_mount_guesses(initial, transforms,
|
||||||
|
observations=self.original_observations)
|
||||||
|
return initial
|
||||||
|
|
||||||
|
|
||||||
|
def fit_draft(bundle, candidate_completed=lambda _row: None):
|
||||||
|
"""Training-only representative; image precision is reported, not certified."""
|
||||||
|
candidates, audit = [], []
|
||||||
|
seed_width = min(6, len(bundle.tags))
|
||||||
|
for seed in product((0, 1), repeat=seed_width):
|
||||||
|
row = dict(seed=list(seed), converged=False)
|
||||||
|
try:
|
||||||
|
fit = bundle.fit(seed)
|
||||||
|
if not fit.success or not np.all(np.isfinite(fit.x)) or not np.all(np.isfinite(fit.fun)):
|
||||||
|
raise ValueError('draft_optimizer_did_not_converge')
|
||||||
|
bundle.output_mappings(fit.x)
|
||||||
|
residual = bundle.original_residual(fit.x) if hasattr(bundle, 'original_residual') else fit.fun
|
||||||
|
errors = np.sqrt(np.mean(np.sum(residual.reshape(-1, 4, 2)**2, axis=-1), axis=-1))
|
||||||
|
row.update(converged=True, coordinate_rms_px=float(np.sqrt(np.mean(residual**2))),
|
||||||
|
tag_rms_p95_px=float(np.percentile(errors, 95)), maximum_tag_rms_px=float(errors.max()),
|
||||||
|
original_image_precision_passed=bool(errors.max() <= 1.5),
|
||||||
|
evaluations=int(fit.nfev+fit.initialization_evaluations))
|
||||||
|
candidates.append((row, fit.x.copy()))
|
||||||
|
except (ValueError, np.linalg.LinAlgError) as error:
|
||||||
|
row['reason'] = str(error)
|
||||||
|
audit.append(row)
|
||||||
|
candidate_completed(row)
|
||||||
|
# A reviewable draft needs a bounded representative search, not a
|
||||||
|
# release claim that all geometric alternatives have been excluded.
|
||||||
|
if len(candidates) == 4:
|
||||||
|
break
|
||||||
|
if not candidates:
|
||||||
|
raise ValueError('draft_no_converged_training_candidate')
|
||||||
|
selected, parameters = min(candidates, key=lambda pair: pair[0]['coordinate_rms_px'])
|
||||||
|
jacobian = bundle.jacobian(parameters)*bundle.scale[None, :]
|
||||||
|
_u, singular, vectors = np.linalg.svd(jacobian, full_matrices=jacobian.shape[0] < jacobian.shape[1])
|
||||||
|
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
|
||||||
|
rank = int(np.count_nonzero(singular > singular[0]*max(rows, jacobian.shape[1])*np.finfo(float).eps))
|
||||||
|
nullspace = vectors[rank:]
|
||||||
|
unresolved = [name for name, column in bundle.zero_columns.items()
|
||||||
|
if len(nullspace) and np.linalg.norm(nullspace[:, column]) > 1e-6]
|
||||||
|
if unresolved:
|
||||||
|
raise ValueError('draft_estimated_zero_unobservable:' + ','.join(unresolved))
|
||||||
|
zeros = {name: dict(estimated_deg=float(np.degrees(bundle.zero_offsets(parameters)[name])),
|
||||||
|
candidate_spread_deg=float(np.degrees(np.ptp([bundle.zero_offsets(value)[name]
|
||||||
|
for _, value in candidates])))) for name in bundle.zero_columns}
|
||||||
|
return parameters, dict(selected_seed=selected['seed'], selection='minimum_training_coordinate_rms',
|
||||||
|
validation_used_for_selection=False, precision_certified=False, candidates=audit,
|
||||||
|
solver_stopping_ftol=getattr(bundle, 'solver_ftol', 1e-9),
|
||||||
|
candidate_search_policy='uncertified_draft_up_to_four_converged_v1',
|
||||||
|
initializations_evaluated=len(audit), available_initializations=2**seed_width,
|
||||||
|
candidate_search_exhaustive=len(audit) == 2**seed_width,
|
||||||
|
estimated_zeros=zeros, local_parameter_rank=rank, parameter_count=len(bundle.scale)), candidates
|
||||||
+6
-6
@@ -11,12 +11,11 @@ def angle_derivatives(bundle, commands, directions):
|
|||||||
jac = np.zeros((len(commands), width))
|
jac = np.zeros((len(commands), width))
|
||||||
if name in bundle.zero_columns:
|
if name in bundle.zero_columns:
|
||||||
jac[:, bundle.zero_columns[name]] = 1.
|
jac[:, bundle.zero_columns[name]] = 1.
|
||||||
basis = bundle.curve_basis(name, commands[:,channel])
|
weights = bundle.curve_weights(name, commands[:,channel], directions[:,channel])
|
||||||
for direction in range(2):
|
for direction in range(2):
|
||||||
weight = np.where(directions[:,channel] == -1, .5, directions[:,channel] == direction)
|
|
||||||
for node, column in enumerate(bundle.curve_columns[name][direction]):
|
for node, column in enumerate(bundle.curve_columns[name][direction]):
|
||||||
if column >= 0:
|
if column >= 0:
|
||||||
jac[:,column] += weight*basis[:,node]
|
jac[:,column] += weights[:,direction,node]
|
||||||
derivatives[bundle.cad_names[name]] = jac
|
derivatives[bundle.cad_names[name]] = jac
|
||||||
return derivatives
|
return derivatives
|
||||||
|
|
||||||
@@ -62,11 +61,12 @@ def physical_intervals(bundle, candidate):
|
|||||||
for i, name in enumerate(bundle.joints):
|
for i, name in enumerate(bundle.joints):
|
||||||
if name in bundle.zero_columns:
|
if name in bundle.zero_columns:
|
||||||
zeros[i,bundle.zero_columns[name]] = 1.
|
zeros[i,bundle.zero_columns[name]] = 1.
|
||||||
basis = bundle.curve_basis(name, bundle.knots[name])
|
for direction in (0, 1):
|
||||||
for columns in bundle.curve_columns[name]:
|
basis = bundle.curve_weights(name, bundle.knots[name],
|
||||||
|
np.full(len(bundle.knots[name]), direction))
|
||||||
for weights in basis:
|
for weights in basis:
|
||||||
row = zeros[i].copy()
|
row = zeros[i].copy()
|
||||||
for column, weight in zip(columns, weights):
|
for column, weight in zip(bundle.curve_columns[name].ravel(), weights.ravel()):
|
||||||
if column >= 0:
|
if column >= 0:
|
||||||
row[column] += weight
|
row[column] += weight
|
||||||
commands.append(row)
|
commands.append(row)
|
||||||
|
|||||||
@@ -0,0 +1,88 @@
|
|||||||
|
"""Constrained batch updates of the existing image objective, without motion.
|
||||||
|
|
||||||
|
The caller supplies the new batch's camera geometry and immutable reference
|
||||||
|
parameters. Unselected joints and tag installations cannot move in the fit.
|
||||||
|
"""
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
|
||||||
|
def check_update_zero_observability(bundle, parameters, active, joints):
|
||||||
|
"""Reject zero gauges, conditional on the explicitly frozen reference."""
|
||||||
|
jacobian = bundle.jacobian(parameters)[:, active]*bundle.scale[active]
|
||||||
|
_, singular, vectors = np.linalg.svd(jacobian,
|
||||||
|
full_matrices=jacobian.shape[0] < jacobian.shape[1])
|
||||||
|
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
|
||||||
|
rank = int(np.count_nonzero(singular > singular[0]*max(rows, len(active))*np.finfo(float).eps))
|
||||||
|
nullspace = vectors[rank:]
|
||||||
|
positions = {column: i for i, column in enumerate(active)}
|
||||||
|
unresolved = [name for name in joints if name in bundle.zero_columns
|
||||||
|
and (bundle.zero_columns[name] not in positions or (len(nullspace)
|
||||||
|
and np.linalg.norm(nullspace[:, positions[bundle.zero_columns[name]]]) > 1e-6))]
|
||||||
|
if unresolved:
|
||||||
|
raise ValueError('batch_update_zero_unobservable:' + ','.join(unresolved))
|
||||||
|
return rank
|
||||||
|
|
||||||
|
|
||||||
|
def transfer_parameters(source, parameters, target):
|
||||||
|
"""Copy semantic parameters; array positions may differ between scopes."""
|
||||||
|
result = np.zeros(len(target.scale))
|
||||||
|
result[:6] = parameters[:6]
|
||||||
|
for name, columns in target.curve_columns.items():
|
||||||
|
if not np.array_equal(source.knots[name], target.knots[name]):
|
||||||
|
raise ValueError('batch_update_training_knots_changed:' + name)
|
||||||
|
mask = columns >= 0
|
||||||
|
original = source.curve_columns[name]
|
||||||
|
if not np.array_equal(mask, original >= 0):
|
||||||
|
raise ValueError('batch_update_baseline_changed:' + name)
|
||||||
|
result[columns[mask]] = parameters[original[mask]]
|
||||||
|
for name, column in target.zero_columns.items():
|
||||||
|
result[column] = source.zero_offsets(parameters)[name]
|
||||||
|
for role, column in target.mount_columns.items():
|
||||||
|
if source.tag_links[role] != target.tag_links[role]:
|
||||||
|
raise ValueError('batch_update_tag_link_changed:' + role)
|
||||||
|
original = source.mount_columns[role]
|
||||||
|
result[column:column+6] = parameters[original:original+6]
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def fit_joint_update(bundle, initial, joints, mount_roles, *, fit_palm=True):
|
||||||
|
"""Use the shared bounded solver while keeping the reference hand fixed."""
|
||||||
|
if not set(joints) <= set(bundle.joints) or not set(mount_roles) <= set(bundle.tags):
|
||||||
|
raise ValueError('batch_update_scope_invalid')
|
||||||
|
columns = list(range(6)) if fit_palm else []
|
||||||
|
for name in joints:
|
||||||
|
if name in bundle.zero_columns:
|
||||||
|
columns.append(bundle.zero_columns[name])
|
||||||
|
columns.extend(bundle.curve_columns[name][bundle.curve_columns[name] >= 0])
|
||||||
|
for role in mount_roles:
|
||||||
|
start = bundle.mount_columns[role]
|
||||||
|
columns.extend(range(start, start+6))
|
||||||
|
proposed = np.asarray(sorted(set(columns)), dtype=int)
|
||||||
|
# Unobserved increments cannot acquire invented travel through optimizer
|
||||||
|
# drift. Retain their input values and report that they were not fitted.
|
||||||
|
supported = np.linalg.norm(bundle.jacobian(initial)[:, proposed], axis=0) > 1e-8
|
||||||
|
active = proposed[supported]
|
||||||
|
if not len(active):
|
||||||
|
raise ValueError('batch_update_no_observed_parameters')
|
||||||
|
def unpack(values):
|
||||||
|
parameters = initial.copy()
|
||||||
|
parameters[active] = values
|
||||||
|
return parameters
|
||||||
|
options = dict(bundle.solver_options)
|
||||||
|
lower, upper = options.pop('bounds')
|
||||||
|
fit = least_squares(lambda x: bundle.residual(unpack(x)), initial[active],
|
||||||
|
jac=lambda x: bundle.solver_jacobian(unpack(x))[:, active],
|
||||||
|
bounds=(lower[active], upper[active]), x_scale=bundle.scale[active],
|
||||||
|
max_nfev=100, ftol=bundle.solver_ftol, **options)
|
||||||
|
parameters = unpack(fit.x)
|
||||||
|
rank = check_update_zero_observability(bundle, parameters, active, joints)
|
||||||
|
derived = {row['column'] for rows in getattr(bundle, 'turnaround_constraints', {}).values() for row in rows}
|
||||||
|
audit = dict(converged=bool(fit.success), evaluations=int(fit.nfev),
|
||||||
|
coordinate_rms_px=float(np.sqrt(np.mean(bundle.original_residual(parameters)**2))),
|
||||||
|
active_columns=active.tolist(),
|
||||||
|
derived_turnaround_columns=sorted(derived.intersection(proposed.tolist())),
|
||||||
|
unobserved_retained_columns=[int(column) for column in proposed[~supported] if column not in derived],
|
||||||
|
validation_used=False, fixed_cad_axes=True, conditional_parameter_rank=rank,
|
||||||
|
observability_condition='unselected_joints_and_tag_mounts_fixed')
|
||||||
|
return parameters, audit
|
||||||
+7
-3
@@ -98,7 +98,11 @@ def _measure_group(bundle, candidate, rows, names, roles, cancelled):
|
|||||||
return fit.x.reshape(count,width), forward, measured, confidence, predicted_poses, measured_poses
|
return fit.x.reshape(count,width), forward, measured, confidence, predicted_poses, measured_poses
|
||||||
|
|
||||||
|
|
||||||
def validate_raw_joint_candidate(bundle, candidate, rows, *, cancelled=lambda: False):
|
def validate_raw_joint_candidate(bundle, candidate, rows, *, cancelled=lambda: False,
|
||||||
|
report_joints=None):
|
||||||
|
names_to_report = tuple(bundle.joints if report_joints is None else report_joints)
|
||||||
|
if not names_to_report or not set(names_to_report) <= set(bundle.joints):
|
||||||
|
raise ValueError('raw_joint_holdout_report_scope_invalid')
|
||||||
angles, positions, rotations = defaultdict(list), defaultdict(list), defaultdict(list)
|
angles, positions, rotations = defaultdict(list), defaultdict(list), defaultdict(list)
|
||||||
primary_counts, pixels, forward_pixels, uncertainty = defaultdict(int), [], [], []
|
primary_counts, pixels, forward_pixels, uncertainty = defaultdict(int), [], [], []
|
||||||
for (view,names,roles), group in _groups(bundle,rows).items():
|
for (view,names,roles), group in _groups(bundle,rows).items():
|
||||||
@@ -118,7 +122,7 @@ def validate_raw_joint_candidate(bundle, candidate, rows, *, cancelled=lambda: F
|
|||||||
positions[name].extend(np.linalg.norm(a[:,:3,3]-b[:,:3,3],axis=1).tolist())
|
positions[name].extend(np.linalg.norm(a[:,:3,3]-b[:,:3,3],axis=1).tolist())
|
||||||
rotations[name].extend(Rotation.from_matrix(a[:,:3,:3].transpose(0,2,1)@b[:,:3,:3]).magnitude().tolist())
|
rotations[name].extend(Rotation.from_matrix(a[:,:3,:3].transpose(0,2,1)@b[:,:3,:3]).magnitude().tolist())
|
||||||
reports = {}
|
reports = {}
|
||||||
for name in bundle.joints:
|
for name in names_to_report:
|
||||||
if primary_counts[name] < bundle.profile.acquisition.minimum_valid_samples:
|
if primary_counts[name] < bundle.profile.acquisition.minimum_valid_samples:
|
||||||
raise ValueError('raw_joint_holdout_measurements_missing:'+name)
|
raise ValueError('raw_joint_holdout_measurements_missing:'+name)
|
||||||
angular,orientation = angular_metrics(angles[name]),angular_metrics(rotations[name])
|
angular,orientation = angular_metrics(angles[name]),angular_metrics(rotations[name])
|
||||||
@@ -129,7 +133,7 @@ def validate_raw_joint_candidate(bundle, candidate, rows, *, cancelled=lambda: F
|
|||||||
passed = (bool(reports) and all(r['passed'] for r in reports.values())
|
passed = (bool(reports) and all(r['passed'] for r in reports.values())
|
||||||
and max(uncertainty,default=np.inf) <= np.radians(1)
|
and max(uncertainty,default=np.inf) <= np.radians(1)
|
||||||
and max(pixels,default=np.inf) <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px)
|
and max(pixels,default=np.inf) <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px)
|
||||||
return dict(passed=passed,joints=reports,
|
return dict(passed=bool(passed),joints=reports,
|
||||||
maximum_measurement_confidence_rad=max(uncertainty,default=None),
|
maximum_measurement_confidence_rad=max(uncertainty,default=None),
|
||||||
maximum_measurement_reprojection_px=max(pixels,default=None),
|
maximum_measurement_reprojection_px=max(pixels,default=None),
|
||||||
frozen_forward_reprojection_p95_px=float(np.percentile(forward_pixels,95)) if forward_pixels else None,
|
frozen_forward_reprojection_p95_px=float(np.percentile(forward_pixels,95)) if forward_pixels else None,
|
||||||
|
|||||||
+20
-4
@@ -182,11 +182,27 @@ class FingerChainImages:
|
|||||||
|
|
||||||
def fit(self, branches):
|
def fit(self, branches):
|
||||||
initial = self.initialize(branches)
|
initial = self.initialize(branches)
|
||||||
|
columns = getattr(self, 'optimization_columns', np.arange(len(initial)))
|
||||||
|
def unpack(values):
|
||||||
|
parameters = initial.copy()
|
||||||
|
parameters[columns] = values
|
||||||
|
return parameters
|
||||||
|
jacobian = getattr(self, 'solver_jacobian', getattr(self, 'jacobian', '2-point'))
|
||||||
|
options = dict(getattr(self, 'solver_options', {}))
|
||||||
|
if 'bounds' in options:
|
||||||
|
options['bounds'] = tuple(np.broadcast_to(bound, initial.shape)[columns]
|
||||||
|
for bound in options['bounds'])
|
||||||
# Small shared graph: exact solver avoids LSMR conditioning ambiguity.
|
# Small shared graph: exact solver avoids LSMR conditioning ambiguity.
|
||||||
result = least_squares(self.residual, initial, x_scale=self.scale,
|
result = least_squares(lambda values: self.residual(unpack(values)), initial[columns],
|
||||||
jac=getattr(self, 'solver_jacobian', getattr(self, 'jacobian', '2-point')),
|
x_scale=self.scale[columns],
|
||||||
**getattr(self, 'solver_options', {}),
|
jac=(lambda values: jacobian(unpack(values))[:, columns]) if callable(jacobian) else jacobian,
|
||||||
max_nfev=getattr(self, 'maximum_joint_evaluations', 200), ftol=1e-9, xtol=1e-9, gtol=1e-7)
|
**options,
|
||||||
|
max_nfev=getattr(self, 'maximum_joint_evaluations', 200),
|
||||||
|
ftol=getattr(self, 'solver_ftol', 1e-9), xtol=1e-9, gtol=1e-7)
|
||||||
|
result.x = unpack(result.x)
|
||||||
|
if len(columns) != len(initial):
|
||||||
|
# Consumers use the stable semantic layout, not optimizer indices.
|
||||||
|
result.jac = jacobian(result.x)
|
||||||
result.initialization_evaluations = getattr(self, '_initialization_costs', {}).get(tuple(branches), 0)
|
result.initialization_evaluations = getattr(self, '_initialization_costs', {}).get(tuple(branches), 0)
|
||||||
return result
|
return result
|
||||||
|
|
||||||
|
|||||||
+38
-26
@@ -217,6 +217,41 @@ def motion_frame_rejection(frame: MotionEvidenceFrame, relations: Sequence[Motio
|
|||||||
return ""
|
return ""
|
||||||
|
|
||||||
|
|
||||||
|
def connect_candidate_families(items, *, strict=True):
|
||||||
|
"""Associate candidate identities, never their per-image reprojection ranks.
|
||||||
|
|
||||||
|
Missing alternatives remain missing. Non-strict association is only a
|
||||||
|
numerical initializer; it must never authorize a branch or certify motion.
|
||||||
|
"""
|
||||||
|
# Anchor where both eligible roots are available. If an alternative
|
||||||
|
# temporarily fails the image-quality gate it stays missing; never copy
|
||||||
|
# the surviving root into both families to invent evidence.
|
||||||
|
anchor = max(range(len(items)), key=lambda i: len(items[i].candidates))
|
||||||
|
count = len(items[anchor].candidates)
|
||||||
|
families = [[None] * len(items) for _ in range(count)]
|
||||||
|
for index, candidate in enumerate(items[anchor].candidates):
|
||||||
|
families[index][anchor] = candidate
|
||||||
|
for sequence in (range(anchor + 1, len(items)), range(anchor - 1, -1, -1)):
|
||||||
|
previous = list(items[anchor].candidates)
|
||||||
|
for frame_index in sequence:
|
||||||
|
current = items[frame_index].candidates
|
||||||
|
def cost(order):
|
||||||
|
return sum(_distance(previous[family], candidate)[0]
|
||||||
|
for family, candidate in zip(order, current))
|
||||||
|
orders = sorted(permutations(range(count), len(current)), key=cost)
|
||||||
|
if (strict and len(orders) == 2 and abs(cost(orders[0]) - cost(orders[1])) < 1e-9
|
||||||
|
and not _equivalent(previous[0], previous[1])):
|
||||||
|
return None, "motion_candidate_assignment_ambiguous"
|
||||||
|
for family, candidate in zip(orders[0], current):
|
||||||
|
angle, translation = _distance(previous[family], candidate)
|
||||||
|
p = DEFAULT_POSE_TRACKING_PARAMETERS
|
||||||
|
if strict and (angle > p.maximum_pose_jump_rad or translation > p.maximum_translation_jump_m):
|
||||||
|
return None, "motion_candidate_family_discontinuous"
|
||||||
|
families[family][frame_index] = candidate
|
||||||
|
previous[family] = candidate
|
||||||
|
return families, ""
|
||||||
|
|
||||||
|
|
||||||
def _connect_paths(frames: Sequence[MotionEvidenceFrame], roles: Sequence[str]):
|
def _connect_paths(frames: Sequence[MotionEvidenceFrame], roles: Sequence[str]):
|
||||||
"""Preserve both continuous families without treating continuity as truth."""
|
"""Preserve both continuous families without treating continuity as truth."""
|
||||||
candidates = [_candidate_map(frame.roles, roles) for frame in frames]
|
candidates = [_candidate_map(frame.roles, roles) for frame in frames]
|
||||||
@@ -227,32 +262,9 @@ def _connect_paths(frames: Sequence[MotionEvidenceFrame], roles: Sequence[str]):
|
|||||||
items = [frame[role] for frame in candidates]
|
items = [frame[role] for frame in candidates]
|
||||||
if any(item.frozen != items[0].frozen for item in items):
|
if any(item.frozen != items[0].frozen for item in items):
|
||||||
return None, "motion_candidate_family_interrupted"
|
return None, "motion_candidate_family_interrupted"
|
||||||
# Anchor where both eligible roots are available. If an alternative
|
families, reason = connect_candidate_families(items)
|
||||||
# temporarily fails the image-quality gate it stays missing; never copy
|
if reason:
|
||||||
# the surviving root into both families to invent evidence.
|
return None, reason
|
||||||
anchor = max(range(len(items)), key=lambda i: len(items[i].candidates))
|
|
||||||
count = len(items[anchor].candidates)
|
|
||||||
families = [[None] * len(frames) for _ in range(count)]
|
|
||||||
for index, candidate in enumerate(items[anchor].candidates):
|
|
||||||
families[index][anchor] = candidate
|
|
||||||
for sequence in (range(anchor + 1, len(frames)), range(anchor - 1, -1, -1)):
|
|
||||||
previous = list(items[anchor].candidates)
|
|
||||||
for frame_index in sequence:
|
|
||||||
current = items[frame_index].candidates
|
|
||||||
def cost(order):
|
|
||||||
return sum(_distance(previous[family], candidate)[0]
|
|
||||||
for family, candidate in zip(order, current))
|
|
||||||
orders = sorted(permutations(range(count), len(current)), key=cost)
|
|
||||||
if (len(orders) == 2 and abs(cost(orders[0]) - cost(orders[1])) < 1e-9
|
|
||||||
and not _equivalent(previous[0], previous[1])):
|
|
||||||
return None, "motion_candidate_assignment_ambiguous"
|
|
||||||
for family, candidate in zip(orders[0], current):
|
|
||||||
angle, translation = _distance(previous[family], candidate)
|
|
||||||
p = DEFAULT_POSE_TRACKING_PARAMETERS
|
|
||||||
if angle > p.maximum_pose_jump_rad or translation > p.maximum_translation_jump_m:
|
|
||||||
return None, "motion_candidate_family_discontinuous"
|
|
||||||
families[family][frame_index] = candidate
|
|
||||||
previous[family] = candidate
|
|
||||||
paths[role] = families
|
paths[role] = families
|
||||||
# Near-coincident IPPE roots represent one physical hypothesis, not two.
|
# Near-coincident IPPE roots represent one physical hypothesis, not two.
|
||||||
for role, families in paths.items():
|
for role, families in paths.items():
|
||||||
|
|||||||
+83
-23
@@ -10,8 +10,9 @@ from scipy.spatial.transform import Rotation
|
|||||||
|
|
||||||
from .finger_chain_images import FingerChainImages
|
from .finger_chain_images import FingerChainImages
|
||||||
from .ippe import solve_square_tag_ippe, square_object_points
|
from .ippe import solve_square_tag_ippe, square_object_points
|
||||||
from .motion_evidence import _axis
|
from .motion_evidence import _axis, RoleCandidates, connect_candidate_families
|
||||||
from ...domain.capture_plan import task_for_joint
|
from ...domain.capture_plan import task_for_joint
|
||||||
|
from ...domain.motion_path import task_segment
|
||||||
from ...domain.sampling import command_nodes
|
from ...domain.sampling import command_nodes
|
||||||
from ...domain.result import JointMapping
|
from ...domain.result import JointMapping
|
||||||
from ...fitting.motion_fit import channel_for_joint, joint_input_direction
|
from ...fitting.motion_fit import channel_for_joint, joint_input_direction
|
||||||
@@ -119,6 +120,10 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
self.maximum_joint_evaluations = 200-5*len(self.tags)
|
self.maximum_joint_evaluations = 200-5*len(self.tags)
|
||||||
self.chains = {role: model._chain(model.parent_joint_by_child[link])
|
self.chains = {role: model._chain(model.parent_joint_by_child[link])
|
||||||
for role, link in self.tag_links.items()}
|
for role, link in self.tag_links.items()}
|
||||||
|
from ...fitting.command_support import compile_turnaround_constraints
|
||||||
|
self.turnaround_constraints = compile_turnaround_constraints(self)
|
||||||
|
derived = {row['column'] for rows in self.turnaround_constraints.values() for row in rows}
|
||||||
|
self.optimization_columns = np.asarray([i for i in range(len(scale)) if i not in derived])
|
||||||
|
|
||||||
def _directions(self, rows):
|
def _directions(self, rows):
|
||||||
result = []
|
result = []
|
||||||
@@ -137,8 +142,24 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
mask = columns >= 0
|
mask = columns >= 0
|
||||||
values[mask] = parameters[columns[mask]]
|
values[mask] = parameters[columns[mask]]
|
||||||
result[name] = values @ self.curve_basis(name, self.knots[name]).T
|
result[name] = values @ self.curve_basis(name, self.knots[name]).T
|
||||||
|
for row in self.turnaround_constraints.get(name, ()):
|
||||||
|
result[name][row['recipient'], row['node']] = result[name][row['donor'], row['node']]
|
||||||
return result
|
return result
|
||||||
|
|
||||||
|
def curve_weights(self, name, commands, directions):
|
||||||
|
"""Derivative of the constrained curves in the original parameter layout."""
|
||||||
|
basis = self.curve_basis(name, commands)
|
||||||
|
weights = np.stack([basis*np.where(directions == -1, .5,
|
||||||
|
directions == direction)[:, None] for direction in (0, 1)], axis=1)
|
||||||
|
for row in self.turnaround_constraints.get(name, ()):
|
||||||
|
hat = np.zeros(len(self.knots[name])); hat[row['node']] = 1.
|
||||||
|
weight = np.interp(commands, self.knots[name], hat)*np.where(
|
||||||
|
directions == -1, .5, directions == row['recipient'])
|
||||||
|
endpoint = self.curve_basis(name, [row['command']])[0]
|
||||||
|
weights[:, row['recipient']] -= weight[:, None]*endpoint
|
||||||
|
weights[:, row['donor']] += weight[:, None]*endpoint
|
||||||
|
return weights
|
||||||
|
|
||||||
def curve_basis(self, name, commands):
|
def curve_basis(self, name, commands):
|
||||||
"""Nonnegative increments enforce the declared native SDK direction."""
|
"""Nonnegative increments enforce the declared native SDK direction."""
|
||||||
knots = self.knots[name]
|
knots = self.knots[name]
|
||||||
@@ -260,16 +281,15 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
if name in self.zero_columns:
|
if name in self.zero_columns:
|
||||||
jac[output_rows, self.zero_columns[name]] = pixel_velocity
|
jac[output_rows, self.zero_columns[name]] = pixel_velocity
|
||||||
channel = self.channels[name]
|
channel = self.channels[name]
|
||||||
basis = self.curve_basis(name, self.commands[indices, channel])
|
weights = self.curve_weights(name, self.commands[indices, channel],
|
||||||
|
self.directions[indices, channel])
|
||||||
for direction in range(2):
|
for direction in range(2):
|
||||||
weight = np.where(self.directions[indices, channel] == -1, .5,
|
|
||||||
(self.directions[indices, channel] == direction).astype(float))
|
|
||||||
for node, column in enumerate(self.curve_columns[name][direction]):
|
for node, column in enumerate(self.curve_columns[name][direction]):
|
||||||
if column >= 0:
|
if column >= 0:
|
||||||
jac[output_rows, column] += pixel_velocity*(weight*basis[:,node])[:,None]
|
jac[output_rows, column] += pixel_velocity*weights[:,direction,node,None]
|
||||||
return jac
|
return jac
|
||||||
|
|
||||||
def _pose(self, index, role, branch):
|
def _pose_candidates(self, index, role):
|
||||||
key = index, role
|
key = index, role
|
||||||
if key not in self._pnps:
|
if key not in self._pnps:
|
||||||
row, tag = self.rows[index], self.tags[role][1]
|
row, tag = self.rows[index], self.tags[role][1]
|
||||||
@@ -278,12 +298,53 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
candidates = self._pnps[key]
|
candidates = self._pnps[key]
|
||||||
if not candidates:
|
if not candidates:
|
||||||
raise ValueError('urdf_images_initialization_candidates_missing:' + role)
|
raise ValueError('urdf_images_initialization_candidates_missing:' + role)
|
||||||
pose = candidates[min(branch, len(candidates)-1)]
|
return candidates
|
||||||
|
|
||||||
|
def _pose_matrix(self, role, pose):
|
||||||
camera = np.eye(4)
|
camera = np.eye(4)
|
||||||
camera[:3, :3] = Rotation.from_quat(pose.quaternion_xyzw).as_matrix()
|
camera[:3, :3] = Rotation.from_quat(pose.quaternion_xyzw).as_matrix()
|
||||||
camera[:3, 3] = pose.translation_xyz_m
|
camera[:3, 3] = pose.translation_xyz_m
|
||||||
return self.extrinsics.transform(self.tags[role][0]) @ camera
|
return self.extrinsics.transform(self.tags[role][0]) @ camera
|
||||||
|
|
||||||
|
def _pose(self, index, role, branch):
|
||||||
|
candidates = self._pose_candidates(index, role)
|
||||||
|
return self._pose_matrix(role, candidates[min(branch, len(candidates)-1)])
|
||||||
|
|
||||||
|
def motion_indices(self, name, direction, cycle):
|
||||||
|
"""Scan membership includes its start, regardless of incoming history.
|
||||||
|
|
||||||
|
Actual per-channel history remains the forward model's hysteresis
|
||||||
|
input. Constant channels in another joint's support segment are omitted.
|
||||||
|
"""
|
||||||
|
task = task_for_joint(self.profile, name)
|
||||||
|
spec = self.profile.measurement.measurements[name]
|
||||||
|
channel = self.channels[name]
|
||||||
|
indices = []
|
||||||
|
for i, row in enumerate(self.rows):
|
||||||
|
if (row['task_name'] != task.key or row['view'] != spec.view
|
||||||
|
or row['cycle'] != cycle or spec.child_role not in row.get('tags', {})):
|
||||||
|
continue
|
||||||
|
if task.segments:
|
||||||
|
segment = task_segment(task, row.get('segment_key', ''))
|
||||||
|
if name not in segment.joints or channel not in segment.moving_channels:
|
||||||
|
continue
|
||||||
|
increasing = dict(segment.end_commands)[channel] > dict(segment.start_commands)[channel]
|
||||||
|
else:
|
||||||
|
increasing = row['direction'] == 'increasing'
|
||||||
|
if int(not increasing) == direction:
|
||||||
|
indices.append(i)
|
||||||
|
return sorted(indices, key=lambda i: self.commands[i, channel])
|
||||||
|
|
||||||
|
def motion_poses(self, indices, role, branch):
|
||||||
|
items = [RoleCandidates(role, self._pose_candidates(i, role)) for i in indices]
|
||||||
|
families, _ = connect_candidate_families(items, strict=False)
|
||||||
|
# A single available family is one seed, not evidence of a second one.
|
||||||
|
family = families[min(branch, len(families)-1)]
|
||||||
|
observed = [(i, pose) for i, pose in zip(indices, family) if pose is not None]
|
||||||
|
if len(observed) < 3:
|
||||||
|
raise ValueError('urdf_images_motion_family_incomplete:' + role)
|
||||||
|
return [i for i, _ in observed], np.asarray([self._pose_matrix(role, p) for _, p in observed])
|
||||||
|
|
||||||
def initialize(self, branches):
|
def initialize(self, branches):
|
||||||
if len(branches) != 6 or any(b not in (0, 1) for b in branches):
|
if len(branches) != 6 or any(b not in (0, 1) for b in branches):
|
||||||
raise ValueError('urdf_images_seed_invalid')
|
raise ValueError('urdf_images_seed_invalid')
|
||||||
@@ -304,16 +365,13 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
spec = self.profile.measurement.measurements[name]
|
spec = self.profile.measurement.measurements[name]
|
||||||
channel = self.channels[name]
|
channel = self.channels[name]
|
||||||
for direction in range(2):
|
for direction in range(2):
|
||||||
indices = [i for i, row in enumerate(self.rows) if row['task_name'] == task.key
|
indices = self.motion_indices(name, direction, 0)
|
||||||
and row['view'] == spec.view and row['cycle'] == 0
|
|
||||||
and spec.child_role in row.get('tags', {}) and self.directions[i, channel] == direction]
|
|
||||||
indices.sort(key=lambda i: self.commands[i, channel])
|
|
||||||
if len(indices) < 3:
|
if len(indices) < 3:
|
||||||
raise ValueError('urdf_images_directional_training_missing:' + name)
|
raise ValueError('urdf_images_directional_training_missing:' + name)
|
||||||
# A common motion-curve guess is only a numerical initializer.
|
# A common motion-curve guess is only a numerical initializer.
|
||||||
# Each candidate retains its own installation seeds below;
|
# Each candidate retains its own installation seeds below;
|
||||||
# no PnP branch or zero is frozen by this starting guess.
|
# no PnP branch or zero is frozen by this starting guess.
|
||||||
poses = np.asarray([self._pose(i, spec.child_role, 0) for i in indices])
|
indices, poses = self.motion_poses(indices, spec.child_role, 0)
|
||||||
rotations = Rotation.from_matrix(poses[:, :3, :3])
|
rotations = Rotation.from_matrix(poses[:, :3, :3])
|
||||||
axis, phase = _axis(rotations)
|
axis, phase = _axis(rotations)
|
||||||
sign = joint_input_direction(self.profile, self.model, name)
|
sign = joint_input_direction(self.profile, self.model, name)
|
||||||
@@ -338,13 +396,10 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
task = task_for_joint(self.profile, root)
|
task = task_for_joint(self.profile, root)
|
||||||
spec = self.profile.measurement.measurements[root]
|
spec = self.profile.measurement.measurements[root]
|
||||||
role = spec.child_role
|
role = spec.child_role
|
||||||
indices = [i for i, row in enumerate(self.rows) if row['task_name'] == task.key
|
indices = self.motion_indices(root, 0, 0)
|
||||||
and row['view'] == spec.view and row['cycle'] == 0 and role in row.get('tags', {})
|
|
||||||
and row['direction'] == 'increasing']
|
|
||||||
indices.sort(key=lambda i: self.commands[i, self.channels[root]])
|
|
||||||
if len(indices) < 3:
|
if len(indices) < 3:
|
||||||
raise ValueError('urdf_images_root_observations_missing:' + root)
|
raise ValueError('urdf_images_root_observations_missing:' + root)
|
||||||
poses = np.asarray([self._pose(i, role, branch) for i in indices])
|
indices, poses = self.motion_poses(indices, role, branch)
|
||||||
rotations = Rotation.from_matrix(poses[:, :3, :3])
|
rotations = Rotation.from_matrix(poses[:, :3, :3])
|
||||||
axis, _ = _axis(rotations)
|
axis, _ = _axis(rotations)
|
||||||
progression = (rotations[-1]*rotations[0].inv()).as_rotvec() @ axis
|
progression = (rotations[-1]*rotations[0].inv()).as_rotvec() @ axis
|
||||||
@@ -415,7 +470,7 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
self._initialization_costs[tuple(branches)] = self._refine_mount_guesses(initial, transforms)
|
self._initialization_costs[tuple(branches)] = self._refine_mount_guesses(initial, transforms)
|
||||||
return initial
|
return initial
|
||||||
|
|
||||||
def _refine_mount_guesses(self, initial, transforms):
|
def _refine_mount_guesses(self, initial, transforms, *, observations=None):
|
||||||
"""Bounded block initialization on the SAME training image objective.
|
"""Bounded block initialization on the SAME training image objective.
|
||||||
|
|
||||||
Five evaluations per installation plus the remaining joint evaluations
|
Five evaluations per installation plus the remaining joint evaluations
|
||||||
@@ -423,8 +478,9 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
"""
|
"""
|
||||||
from scipy.optimize import least_squares
|
from scipy.optimize import least_squares
|
||||||
evaluations = 0
|
evaluations = 0
|
||||||
|
observations = self.observations if observations is None else observations
|
||||||
for role, column in self.mount_columns.items():
|
for role, column in self.mount_columns.items():
|
||||||
indices, observed, matrices, objects = self.observations[role]
|
indices, observed, matrices, objects = observations[role]
|
||||||
camera = np.linalg.inv(self.extrinsics.transform(self.tags[role][0])) @ transforms[self.tag_links[role]][indices]
|
camera = np.linalg.inv(self.extrinsics.transform(self.tags[role][0])) @ transforms[self.tag_links[role]][indices]
|
||||||
def project(values):
|
def project(values):
|
||||||
mount = rigid(values)
|
mount = rigid(values)
|
||||||
@@ -453,13 +509,17 @@ class UrdfCommandImages(FingerChainImages):
|
|||||||
return evaluations
|
return evaluations
|
||||||
|
|
||||||
def covariance(self, fit):
|
def covariance(self, fit):
|
||||||
jac = (fit.jac.toarray() if hasattr(fit.jac, 'toarray') else np.asarray(fit.jac))*self.scale[None, :]
|
active = self.optimization_columns
|
||||||
|
jac = (fit.jac.toarray() if hasattr(fit.jac, 'toarray') else np.asarray(fit.jac))[:, active]*self.scale[active]
|
||||||
information = jac.T @ jac
|
information = jac.T @ jac
|
||||||
eigen = np.linalg.eigvalsh(information)
|
eigen = np.linalg.eigvalsh(information)
|
||||||
if len(fit.fun) <= len(fit.x) or eigen[0] <= eigen[-1]*np.finfo(float).eps*len(self.scale):
|
if len(fit.fun) <= len(active) or eigen[0] <= eigen[-1]*np.finfo(float).eps*len(active):
|
||||||
raise ValueError('urdf_images_geometry_rank_deficient')
|
raise ValueError('urdf_images_geometry_rank_deficient')
|
||||||
variance = max(.03**2, float(fit.fun @ fit.fun/(len(fit.fun)-len(fit.x))))
|
variance = max(.03**2, float(fit.fun @ fit.fun/(len(fit.fun)-len(active))))
|
||||||
return np.linalg.inv(information)*variance*self.scale[:, None]*self.scale[None, :]
|
covariance = np.zeros((len(self.scale), len(self.scale)))
|
||||||
|
covariance[np.ix_(active, active)] = (np.linalg.inv(information)*variance
|
||||||
|
*self.scale[active, None]*self.scale[None, active])
|
||||||
|
return covariance
|
||||||
|
|
||||||
def output_mappings(self, parameters):
|
def output_mappings(self, parameters):
|
||||||
return {name: JointMapping(name, self.channels[name], 'command_u8', tuple(self.knots[name]),
|
return {name: JointMapping(name, self.channels[name], 'command_u8', tuple(self.knots[name]),
|
||||||
|
|||||||
@@ -31,18 +31,22 @@ class MotionRangeEvidence:
|
|||||||
command_training_cycles: tuple[int, ...] = (0, 1, 2)
|
command_training_cycles: tuple[int, ...] = (0, 1, 2)
|
||||||
command_training_sample_ids: tuple[str, ...] = ()
|
command_training_sample_ids: tuple[str, ...] = ()
|
||||||
release_basis: str = "feedback_and_command"
|
release_basis: str = "feedback_and_command"
|
||||||
|
command_support: dict | None = None
|
||||||
|
|
||||||
|
|
||||||
def _interval(value):
|
def _interval(value):
|
||||||
return len(value) == 2 and all(math.isfinite(v) for v in value) and value[0] < value[1]
|
return len(value) == 2 and all(math.isfinite(v) for v in value) and value[0] < value[1]
|
||||||
|
|
||||||
|
|
||||||
def validate_range_evidence(name, original, delta, interval, evidence):
|
def validate_range_evidence(name, original, delta, interval, evidence, *, allow_single_training_cycle=False):
|
||||||
"""Validate trusted fit evidence at both planning and final read-back."""
|
"""Validate trusted fit evidence at both planning and final read-back."""
|
||||||
|
cycles = {LEGACY_TRAINING, LEGACY_TRAINING[:2]}
|
||||||
|
if allow_single_training_cycle:
|
||||||
|
cycles |= {(0,), (1,)} # Explicit uncertified draft only; never holdout.
|
||||||
if (not isinstance(evidence, MotionRangeEvidence) or evidence.joint != name
|
if (not isinstance(evidence, MotionRangeEvidence) or evidence.joint != name
|
||||||
or evidence.source_limits != "calibrated_motion_envelope" or not evidence.evidence_source.strip()
|
or evidence.source_limits != "calibrated_motion_envelope" or not evidence.evidence_source.strip()
|
||||||
or evidence.training_cycles not in {LEGACY_TRAINING, LEGACY_TRAINING[:2]} or not evidence.training_sample_ids
|
or evidence.training_cycles not in cycles or not evidence.training_sample_ids
|
||||||
or evidence.command_training_cycles not in {LEGACY_TRAINING, LEGACY_TRAINING[:2], (4,)}
|
or evidence.command_training_cycles not in cycles | {(4,)}
|
||||||
or (evidence.command_training_cycles == (4,) and not evidence.command_training_sample_ids)
|
or (evidence.command_training_cycles == (4,) and not evidence.command_training_sample_ids)
|
||||||
or not evidence.reference_sample_ids or not evidence.source_joint
|
or not evidence.reference_sample_ids or not evidence.source_joint
|
||||||
or evidence.independently_measured != (evidence.source_joint == name)
|
or evidence.independently_measured != (evidence.source_joint == name)
|
||||||
|
|||||||
@@ -0,0 +1,45 @@
|
|||||||
|
"""Validate the actual limits-linear SDK driver without refitting any curve."""
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ..geometry.tag_pose.urdf_command_images import UrdfCommandImages
|
||||||
|
from ..fitting.motion_fit import joint_input_direction
|
||||||
|
|
||||||
|
|
||||||
|
class UrdfLinearImages:
|
||||||
|
"""Keep frozen camera/mount geometry, replacing only the angle provider."""
|
||||||
|
def __init__(self, bundle, corrected):
|
||||||
|
self.bundle, self.corrected = bundle, corrected
|
||||||
|
|
||||||
|
def __getattr__(self, name):
|
||||||
|
return getattr(self.bundle, name)
|
||||||
|
|
||||||
|
def joint_angles(self, parameters, commands, directions):
|
||||||
|
offsets = self.bundle.zero_offsets(parameters)
|
||||||
|
angles = {}
|
||||||
|
for name, channel in self.channels.items():
|
||||||
|
cad = self.cad_names[name]
|
||||||
|
joint = self.corrected.joints[cad]
|
||||||
|
ratio = np.asarray(commands[:, channel], dtype=float)/255.
|
||||||
|
if np.any((ratio < 0) | (ratio > 1)):
|
||||||
|
raise ValueError('urdf_linear_command_outside_byte_range')
|
||||||
|
if joint_input_direction(self.profile, self.model, name) < 0:
|
||||||
|
ratio = 1-ratio
|
||||||
|
# The projector keeps the original CAD origin. Account for the
|
||||||
|
# exported origin exactly once, including nonzero byte baselines.
|
||||||
|
angles[cad] = offsets[name]+joint.lower+(joint.upper-joint.lower)*ratio
|
||||||
|
return angles
|
||||||
|
|
||||||
|
def link_poses(self, parameters, commands, directions, angle_corrections=None):
|
||||||
|
return UrdfCommandImages.link_poses(self, parameters, commands, directions, angle_corrections)
|
||||||
|
|
||||||
|
def project_observations(self, parameters, commands, directions, observations, angle_corrections=None):
|
||||||
|
return UrdfCommandImages.project_observations(self, parameters, commands, directions,
|
||||||
|
observations, angle_corrections)
|
||||||
|
|
||||||
|
|
||||||
|
def validate_urdf_linear_drive(bundle, parameters, corrected, holdout, *, report_joints=None):
|
||||||
|
from types import SimpleNamespace
|
||||||
|
from ..fitting.raw_joint_validation import validate_raw_joint_candidate
|
||||||
|
result = validate_raw_joint_candidate(UrdfLinearImages(bundle, corrected),
|
||||||
|
SimpleNamespace(parameters=parameters), holdout, report_joints=report_joints)
|
||||||
|
return dict(result, drive='urdf_limits_linear_sdk_u8', parameters_refitted=False)
|
||||||
@@ -62,6 +62,7 @@ def build_standard_correction_plan(
|
|||||||
independent_joint_channels: Mapping[str, int] | None = None,
|
independent_joint_channels: Mapping[str, int] | None = None,
|
||||||
fitted_mimics_output: Mapping[str, LinearMimicFit] | None = None,
|
fitted_mimics_output: Mapping[str, LinearMimicFit] | None = None,
|
||||||
preserve_source_mimic: bool = False,
|
preserve_source_mimic: bool = False,
|
||||||
|
allow_single_training_cycle: bool = False,
|
||||||
) -> StandardUrdfPlan:
|
) -> StandardUrdfPlan:
|
||||||
"""Generate authorized standard-URDF edits, never clamp an invalid fit.
|
"""Generate authorized standard-URDF edits, never clamp an invalid fit.
|
||||||
|
|
||||||
@@ -133,7 +134,8 @@ def build_standard_correction_plan(
|
|||||||
if len(interval) != 2 or not all(math.isfinite(v) for v in interval) or interval[0] >= interval[1]:
|
if len(interval) != 2 or not all(math.isfinite(v) for v in interval) or interval[0] >= interval[1]:
|
||||||
raise ValueError(f"invalid corrected interval: {name}")
|
raise ValueError(f"invalid corrected interval: {name}")
|
||||||
if name in motion_ranges:
|
if name in motion_ranges:
|
||||||
validate_range_evidence(name, joint, delta, interval, motion_ranges[name])
|
validate_range_evidence(name, joint, delta, interval, motion_ranges[name],
|
||||||
|
allow_single_training_cycle=allow_single_training_cycle)
|
||||||
elif interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9:
|
elif interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9:
|
||||||
raise ValueError(f"corrected range exceeds source mechanical range: {name}")
|
raise ValueError(f"corrected range exceeds source mechanical range: {name}")
|
||||||
if name in envelopes:
|
if name in envelopes:
|
||||||
|
|||||||
@@ -49,13 +49,13 @@ def compile_capture_scope(profile):
|
|||||||
suffix = "_" + name
|
suffix = "_" + name
|
||||||
return value if path.stem.endswith(suffix) else path.stem + suffix + path.suffix
|
return value if path.stem.endswith(suffix) else path.stem + suffix + path.suffix
|
||||||
return replace(profile,
|
return replace(profile,
|
||||||
quality=replace(profile.quality, training_policy='taskwise_2_plus_1')
|
|
||||||
if profile.quality.training_policy == 'raw_joint_2_plus_1' else profile.quality,
|
|
||||||
motion=replace(profile.motion, tasks=tuple(tasks),
|
motion=replace(profile.motion, tasks=tuple(tasks),
|
||||||
joint_zero_references=keep(profile.motion.joint_zero_references)),
|
joint_zero_references=keep(profile.motion.joint_zero_references)),
|
||||||
measurement=replace(profile.measurement, measurements=keep(profile.measurement.measurements)),
|
measurement=replace(profile.measurement, measurements=keep(profile.measurement.measurements)),
|
||||||
zero=replace(zero, direct_zero_joints=tuple(j for j in zero.direct_zero_joints if j not in retained),
|
zero=replace(zero, direct_zero_joints=tuple(j for j in zero.direct_zero_joints if j not in retained),
|
||||||
axis_joints=tuple(j for j in zero.axis_joints if j not in retained),
|
axis_joints=tuple(j for j in zero.axis_joints if j not in retained),
|
||||||
|
cad_zero_assumptions=keep(zero.cad_zero_assumptions),
|
||||||
|
known_baseline_geometry=keep(zero.known_baseline_geometry),
|
||||||
cad_frozen_joints=zero.cad_frozen_joints | retained, spatial=spatial),
|
cad_frozen_joints=zero.cad_frozen_joints | retained, spatial=spatial),
|
||||||
artifacts=replace(profile.artifacts,
|
artifacts=replace(profile.artifacts,
|
||||||
directional_command_joints=profile.artifacts.directional_command_joints - retained,
|
directional_command_joints=profile.artifacts.directional_command_joints - retained,
|
||||||
|
|||||||
@@ -39,7 +39,10 @@ class O30RosAdapter(LegacyByteSdkAdapter):
|
|||||||
or not str(data.get("uid") or "").strip()
|
or not str(data.get("uid") or "").strip()
|
||||||
or tuple(data.get("joint_names", ())) != self.command_layout.names
|
or tuple(data.get("joint_names", ())) != self.command_layout.names
|
||||||
or not isinstance(data.get("online"), bool)):
|
or not isinstance(data.get("online"), bool)):
|
||||||
raise ValueError("o30_sdk_identity_mismatch")
|
observed = ({key: data.get(key) for key in
|
||||||
|
('hand_type', 'model', 'side', 'uid', 'joint_names', 'online')}
|
||||||
|
if isinstance(data, dict) else data)
|
||||||
|
raise ValueError("o30_sdk_identity_mismatch:" + json.dumps(observed, ensure_ascii=False))
|
||||||
if self.info is not None and data["uid"] != self.info["uid"]:
|
if self.info is not None and data["uid"] != self.info["uid"]:
|
||||||
raise ValueError("o30_sdk_device_changed")
|
raise ValueError("o30_sdk_device_changed")
|
||||||
faults = data.get("joint_faults", {})
|
faults = data.get("joint_faults", {})
|
||||||
|
|||||||
+10
-1
@@ -15,9 +15,18 @@ from ..resume import ResumeVerifier, fingerprint_from_mapping
|
|||||||
from ..scan_quality import evaluate_capture_unit
|
from ..scan_quality import evaluate_capture_unit
|
||||||
|
|
||||||
|
|
||||||
def validate_capture_provenance(profile, serial_number, hashes, records):
|
def validate_capture_provenance(profile, serial_number, hashes, records, *, allow_installation_draft=False):
|
||||||
"""A journal cannot substitute today's hashes for old evidence."""
|
"""A journal cannot substitute today's hashes for old evidence."""
|
||||||
reject_diagnostic_capture(records)
|
reject_diagnostic_capture(records)
|
||||||
|
if not allow_installation_draft and any(row.get('kind') == 'raw_tag_remount_checkpoint' for row in records):
|
||||||
|
raise ValueError('capture_provenance:remounted_capture_requires_separate_installation_certification')
|
||||||
|
for row in records:
|
||||||
|
if row.get('kind') == 'raw_resume_verified':
|
||||||
|
check = row.get('installation_verification', {})
|
||||||
|
if (not allow_installation_draft and (check.get('requires_offline_review')
|
||||||
|
or check.get('changed_roles') or check.get('unavailable_roles')
|
||||||
|
or check.get('installation_verified') is False)):
|
||||||
|
raise ValueError('capture_provenance:resume_installation_requires_offline_review')
|
||||||
headers = [row for row in records if row.get("kind") == "session_start"]
|
headers = [row for row in records if row.get("kind") == "session_start"]
|
||||||
references = [row for row in records if row.get("kind") == "fixed_base_reference_locked"]
|
references = [row for row in records if row.get("kind") == "fixed_base_reference_locked"]
|
||||||
if len(headers) != 1 or len(references) != 1:
|
if len(headers) != 1 or len(references) != 1:
|
||||||
|
|||||||
+348
@@ -0,0 +1,348 @@
|
|||||||
|
"""Offline approximate URDF export, separate from precision certification."""
|
||||||
|
from dataclasses import asdict, replace
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from types import SimpleNamespace
|
||||||
|
import traceback
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ...core.domain.result import JointMapping
|
||||||
|
from ...core.fitting.command_support import validate_turnaround_mapping
|
||||||
|
from ...core.fitting.raw_joint_draft import (
|
||||||
|
DraftCommandImages, draft_profile, draft_targets, draft_training_coverage, fit_draft,
|
||||||
|
define_upright_splay_zero, UPRIGHT_SPLAY_JOINTS, UPRIGHT_SPLAY_REFERENCE,
|
||||||
|
)
|
||||||
|
from ...core.fitting.raw_joint_validation import validate_raw_joint_candidate
|
||||||
|
from ...core.geometry.extrinsics import load_camera_extrinsics
|
||||||
|
from ...core.urdf.kinematics import UrdfKinematicModel
|
||||||
|
from ...core.urdf.limits import MotionRangeEvidence
|
||||||
|
from ...core.urdf.plan import build_standard_correction_plan
|
||||||
|
from ...profiles import load_hand_profile
|
||||||
|
from ...profiles.observations import compile_tag_links
|
||||||
|
from ...storage import atomic_write_json
|
||||||
|
from ..acquisition import load_capture
|
||||||
|
from ..engine import CalibrationEngine
|
||||||
|
from ..raw_completion import file_digest, implementation_digest, verify_capture_manifest
|
||||||
|
from ..raw_evidence import partition_raw_capture, fitting_frames
|
||||||
|
from .capture_validation import validate_capture_provenance
|
||||||
|
from .raw_joint_report import prepare_raw_motion_ranges
|
||||||
|
from .serializers.unified_v1 import _mapping
|
||||||
|
from .standard_loader import validate_with_robot_state_publisher
|
||||||
|
from .urdf_from_json import serialize_urdf_correction
|
||||||
|
|
||||||
|
DRAFT_FORMAT = 'raw_joint_urdf_draft_v1'
|
||||||
|
|
||||||
|
|
||||||
|
def evaluate_draft_holdout(bundle, parameters, holdout):
|
||||||
|
"""Preserve evaluator failures without certifying or refitting a draft."""
|
||||||
|
try:
|
||||||
|
return validate_raw_joint_candidate(bundle, SimpleNamespace(parameters=parameters), holdout)
|
||||||
|
except Exception as error:
|
||||||
|
# An evaluator failure is not a measured accuracy result. Retain its
|
||||||
|
# diagnostics; only the uncertified training draft remains exportable.
|
||||||
|
return dict(passed=False, status='evaluation_failed', reason=str(error),
|
||||||
|
exception_type=type(error).__name__, traceback=traceback.format_exc())
|
||||||
|
|
||||||
|
|
||||||
|
def capture_profile_for_draft(profile, header, path):
|
||||||
|
"""Replay may correct model bindings/signs, never the captured SDK route."""
|
||||||
|
if path is None:
|
||||||
|
return profile
|
||||||
|
path = Path(path)
|
||||||
|
if file_digest(path) != header.get('profile_config_sha256'):
|
||||||
|
raise ValueError('draft_capture_profile_hash_mismatch')
|
||||||
|
captured = load_hand_profile(path)
|
||||||
|
current = {tag.role: tag.link for view in profile.vision.views for tag in view.tags}
|
||||||
|
corrected = replace(captured,
|
||||||
|
command=replace(captured.command, sdk_to_joint_direction=profile.command.sdk_to_joint_direction),
|
||||||
|
vision=replace(captured.vision, views=tuple(
|
||||||
|
replace(view, tags=tuple(replace(tag, link=current.get(tag.role, tag.link)) for tag in view.tags))
|
||||||
|
for view in captured.vision.views)))
|
||||||
|
if corrected != profile:
|
||||||
|
raise ValueError('draft_profile_change_must_only_correct_model_bindings_or_directions')
|
||||||
|
return captured
|
||||||
|
|
||||||
|
|
||||||
|
def read_draft_plan(payload, source, profile):
|
||||||
|
"""Rebuild the plan from saved numeric evidence, with the original writer."""
|
||||||
|
if (payload.get('format') != DRAFT_FORMAT or payload.get('profile_id') != profile.key.profile_id
|
||||||
|
or payload.get('accuracy_passed') is not False or payload.get('publication_allowed') is not False):
|
||||||
|
raise ValueError('draft_identity_or_status_invalid')
|
||||||
|
joints = payload['joints']
|
||||||
|
if not joints or not set(joints) <= profile.zero.active_joints:
|
||||||
|
raise ValueError('draft_joint_scope_invalid')
|
||||||
|
if set(payload['motion_range_evidence']) != set(joints):
|
||||||
|
raise ValueError('draft_range_scope_mismatch')
|
||||||
|
ranges, mappings, offsets = {}, {}, {}
|
||||||
|
for name, row in joints.items():
|
||||||
|
table = row['command_to_rad']
|
||||||
|
mappings[name] = JointMapping(name, table['channel_index'], table['input_domain'],
|
||||||
|
*(tuple(table[key]) for key in ('knots', 'angle_rad', 'increasing_rad', 'decreasing_rad')))
|
||||||
|
offsets[name] = float(row['zero_offset_rad'])
|
||||||
|
if (not np.isfinite(offsets[name]) or abs(offsets[name]) > np.radians(20)+1e-10
|
||||||
|
or (row['zero_status'] in ('retained_cad', 'operator_defined') and offsets[name] != 0.)):
|
||||||
|
raise ValueError('draft_zero_outside_authorized_model:' + name)
|
||||||
|
if row['zero_status'] == 'operator_defined' and (name not in UPRIGHT_SPLAY_JOINTS
|
||||||
|
or row.get('zero_assumption') != UPRIGHT_SPLAY_REFERENCE
|
||||||
|
or payload.get('baseline_command') != list(profile.command.baseline_values)):
|
||||||
|
raise ValueError('draft_operator_zero_reference_invalid:' + name)
|
||||||
|
evidence = payload['motion_range_evidence'][name]
|
||||||
|
ranges[name] = MotionRangeEvidence(**{key: tuple(value) if isinstance(value, list) else value
|
||||||
|
for key, value in evidence.items()})
|
||||||
|
validate_turnaround_mapping(mappings[name], ranges[name].command_support)
|
||||||
|
if (mappings[name].motor_index != profile.command.command_index_by_joint[name]
|
||||||
|
or mappings[name].input_domain != 'command_u8'
|
||||||
|
or mappings[name].bounds_rad != ranges[name].command_bounds_rad
|
||||||
|
or abs(mappings[name].evaluate(ranges[name].baseline_command)) > 1e-10):
|
||||||
|
raise ValueError('draft_mapping_differs_from_urdf_evidence:' + name)
|
||||||
|
plan = build_standard_correction_plan(source_urdf=source,
|
||||||
|
source_sha256=payload['urdf_correction']['source_urdf_sha256'], zero_offsets_rad=offsets,
|
||||||
|
measured_ranges_output_rad={name: value.output_limits_rad for name, value in ranges.items()},
|
||||||
|
authorized_fields=profile.urdf_authorized_fields, range_evidence=ranges, preserve_source_mimic=True,
|
||||||
|
allow_single_training_cycle=True)
|
||||||
|
if serialize_urdf_correction(plan) != payload['urdf_correction']:
|
||||||
|
raise ValueError('draft_saved_correction_differs_from_evidence')
|
||||||
|
return plan, mappings
|
||||||
|
|
||||||
|
|
||||||
|
def load_training_initialization(directory, config, input_sha256):
|
||||||
|
"""A prior same-data training result supplies a guess, never a fixed zero."""
|
||||||
|
directory = Path(directory).resolve()
|
||||||
|
paths = [directory/name for name in ('draft_result.json', 'draft_calibration.json',
|
||||||
|
'draft_training.json', 'draft_candidates.npz')]
|
||||||
|
digests = {str(path): file_digest(path) for path in paths}
|
||||||
|
state, payload, training = (json.loads(path.read_text()) for path in paths[:3])
|
||||||
|
from ..runner_support import protected_inputs
|
||||||
|
if (state.get('format') != DRAFT_FORMAT or not state.get('generated')
|
||||||
|
or state.get('input_sha256') != input_sha256
|
||||||
|
or state.get('solver_protected_inputs') != protected_inputs(config)
|
||||||
|
or training.get('validation_used_for_selection') is not False
|
||||||
|
or training.get('selection') != 'minimum_training_coordinate_rms'
|
||||||
|
or payload.get('training') != training):
|
||||||
|
raise ValueError('draft_initialization_must_match_source_and_training_only_selection')
|
||||||
|
_, mappings = read_draft_plan(payload, config.source_urdf, config.calibration_contract.typed_profile)
|
||||||
|
with np.load(paths[3], allow_pickle=False) as arrays:
|
||||||
|
matches = np.flatnonzero(np.all(arrays['seeds'] == training['selected_seed'], axis=1))
|
||||||
|
if len(matches) != 1:
|
||||||
|
raise ValueError('draft_initialization_selected_seed_missing')
|
||||||
|
parameters = arrays['parameters'][matches[0]]
|
||||||
|
if not np.all(np.isfinite(parameters)) or len(parameters) < 6:
|
||||||
|
raise ValueError('draft_initialization_parameters_invalid')
|
||||||
|
hint = dict(palm=parameters[:6], mappings=mappings,
|
||||||
|
offsets={name: float(row['zero_offset_rad']) for name, row in payload['joints'].items()})
|
||||||
|
return hint, dict(source_directory=str(directory), input_sha256=digests,
|
||||||
|
policy='same_capture_training_initial_guess_v1', shared_parameters_frozen=False,
|
||||||
|
validation_used=False)
|
||||||
|
|
||||||
|
|
||||||
|
def export_draft_artifacts(payload, source, profile, directory, probes, *,
|
||||||
|
standard_loader=validate_with_robot_state_publisher):
|
||||||
|
"""One writer for single/multiple-batch drafts; JSON owns every edit."""
|
||||||
|
directory = Path(directory)
|
||||||
|
json_path, urdf_path = directory/'draft_calibration.json', directory/'o30_right_draft.urdf'
|
||||||
|
if json_path.exists() or urdf_path.exists():
|
||||||
|
raise ValueError('draft_artifacts_already_exist')
|
||||||
|
# Check before saving so invalid evidence cannot look like a finished pair.
|
||||||
|
read_draft_plan(payload, source, profile)
|
||||||
|
atomic_write_json(json_path, payload)
|
||||||
|
saved = json.loads(json_path.read_text())
|
||||||
|
plan, mappings = read_draft_plan(saved, source, profile)
|
||||||
|
plan.write(source, urdf_path)
|
||||||
|
original, corrected = UrdfKinematicModel(source), UrdfKinematicModel(urdf_path)
|
||||||
|
offsets = {name: row['zero_offset_rad'] for name, row in saved['joints'].items()}
|
||||||
|
commands = {(tuple(row['command_vector']), tuple(row['command_direction_by_index'])) for row in probes}
|
||||||
|
if not commands:
|
||||||
|
raise ValueError('draft_final_file_probes_missing')
|
||||||
|
maximum = 0.
|
||||||
|
for values, directions in sorted(commands):
|
||||||
|
angles = {name: mapping.evaluate(values[mapping.motor_index], directions[mapping.motor_index])
|
||||||
|
for name, mapping in mappings.items()}
|
||||||
|
for name in mappings:
|
||||||
|
cad = profile.command.urdf_joint_by_joint.get(name, name)
|
||||||
|
before = original.link_transform(cad, zero_offsets=offsets, joint_angles=angles,
|
||||||
|
independent_mimic_angles=True)
|
||||||
|
after = corrected.link_transform(cad, zero_offsets={}, joint_angles=angles,
|
||||||
|
independent_mimic_angles=True)
|
||||||
|
maximum = max(maximum, float(np.max(np.abs(before-after))))
|
||||||
|
if maximum > 1e-8:
|
||||||
|
raise ValueError('draft_json_urdf_fk_mismatch')
|
||||||
|
standard_loader(urdf_path)
|
||||||
|
return json_path, urdf_path, corrected, maximum
|
||||||
|
|
||||||
|
|
||||||
|
def replay_draft(config, raw_path, *, output, capture_profile=None, initialization_from=None,
|
||||||
|
upright_splay_zero=False,
|
||||||
|
standard_loader=validate_with_robot_state_publisher):
|
||||||
|
"""No SDK, resume, publisher or automatic recapture can be reached here."""
|
||||||
|
from ..runner_support import protected_inputs, create_session_directory
|
||||||
|
raw_path = Path(raw_path).resolve()
|
||||||
|
installation_note = raw_path.parent/'operator_installation_clarification.json'
|
||||||
|
files = sorted({raw_path, *raw_path.parent.glob(raw_path.stem + '.*'),
|
||||||
|
*raw_path.parent.glob('imported_evidence_*.zlib'), installation_note})
|
||||||
|
files = [path for path in files if path.exists()]
|
||||||
|
digests = {str(path): file_digest(path) for path in files}
|
||||||
|
records = load_capture(raw_path)
|
||||||
|
header = next(row for row in records if row.get('kind') == 'session_start')
|
||||||
|
profile = config.calibration_contract.typed_profile
|
||||||
|
if profile.key.model != 'O30':
|
||||||
|
raise ValueError('draft_currently_requires_o30_raw_capture')
|
||||||
|
snapshot = raw_path.parent/'capture_profile.yaml'
|
||||||
|
if (capture_profile is None and snapshot.is_file()
|
||||||
|
and header.get('profile_config_sha256') != config.profile_config_sha256):
|
||||||
|
capture_profile = snapshot
|
||||||
|
captured = capture_profile_for_draft(profile, header, capture_profile)
|
||||||
|
hashes = protected_inputs(config)
|
||||||
|
captured_hashes = dict(hashes)
|
||||||
|
if capture_profile is not None:
|
||||||
|
captured_hashes['profile_config_sha256'] = file_digest(Path(capture_profile))
|
||||||
|
validate_capture_provenance(captured, config.serial_number, captured_hashes, records,
|
||||||
|
allow_installation_draft=True)
|
||||||
|
from ..motion_provenance import validate_source_geometry
|
||||||
|
validate_source_geometry(captured, config.source_urdf, records)
|
||||||
|
sealed = (raw_path.parent/'raw_capture_manifest.json').exists()
|
||||||
|
if sealed:
|
||||||
|
manifest = verify_capture_manifest(raw_path)
|
||||||
|
if manifest['capture_plan'] != header['capture_plan'] or manifest['serial_number'] != config.serial_number:
|
||||||
|
raise ValueError('draft_manifest_identity_changed')
|
||||||
|
evidence = partition_raw_capture(captured, records, require_complete=False, reassess_quality=True)
|
||||||
|
training = fitting_frames(captured, evidence.training)
|
||||||
|
model = UrdfKinematicModel(config.source_urdf)
|
||||||
|
captured_links = compile_tag_links(captured, model)
|
||||||
|
corrected_links = compile_tag_links(profile, model)
|
||||||
|
units = CalibrationEngine(captured).scan_units()
|
||||||
|
coverage = draft_training_coverage(evidence.completed_units, units)
|
||||||
|
selected = draft_profile(profile, model, evidence.completed_units, units, training)
|
||||||
|
if upright_splay_zero:
|
||||||
|
selected = define_upright_splay_zero(selected, model)
|
||||||
|
tasks = {task.key for task in selected.motion.tasks}
|
||||||
|
roles = {tag.role for view in selected.vision.views for tag in view.tags if not tag.fixed_reference}
|
||||||
|
def scoped(rows):
|
||||||
|
return tuple(row for row in rows if row['task_name'] in tasks and roles.intersection(row['tags']))
|
||||||
|
training, holdout = scoped(training), scoped(fitting_frames(captured, evidence.validation))
|
||||||
|
# Every accepted image must use the locked rectified camera projection.
|
||||||
|
matrices = {row['view']: row['camera_matrix'] for row in records if row.get('kind') == 'rectified_camera_model'}
|
||||||
|
if any(not np.array_equal(row['camera_matrix'], matrices[row['view']]) for row in (*training, *holdout)):
|
||||||
|
raise ValueError('draft_camera_projection_changed')
|
||||||
|
extrinsics = load_camera_extrinsics(config.camera_extrinsics, required_views=profile.vision.view_names,
|
||||||
|
reference_view=profile.vision.extrinsic_reference_view, quality_limits=profile.vision.extrinsics_quality_limits,
|
||||||
|
minimum_capture_counts=profile.vision.minimum_capture_counts)
|
||||||
|
hint, hint_audit = (load_training_initialization(initialization_from, config, digests)
|
||||||
|
if initialization_from is not None else (None, None))
|
||||||
|
bundle = DraftCommandImages(selected, model, extrinsics, training, compile_tag_links(selected, model),
|
||||||
|
initialization_cycles={task: coverage[task]['initialization_cycle'] for task in tasks},
|
||||||
|
training_initialization=hint)
|
||||||
|
if output is None:
|
||||||
|
directory = create_session_directory(config.session_root/'drafts')
|
||||||
|
else:
|
||||||
|
directory = Path(output).expanduser().resolve()
|
||||||
|
directory.mkdir(parents=True, exist_ok=False)
|
||||||
|
state = dict(format=DRAFT_FORMAT, commands_enabled=False, accuracy_passed=False,
|
||||||
|
publication_allowed=False, generated=False, input_sha256=digests, sealed_capture=sealed,
|
||||||
|
missing_units=evidence.missing_units, raw_capture_complete=not evidence.missing_units,
|
||||||
|
acquisition_reassessment=evidence.reassessed_units, implementation_sha256=implementation_digest(),
|
||||||
|
training_selection=dict(policy='at_least_one_complete_training_cycle_v1',
|
||||||
|
tasks=coverage, included_tasks=sorted(tasks), training_frames=len(training),
|
||||||
|
validation_frames=len(holdout), validation_used_for_training=False),
|
||||||
|
training_initialization=hint_audit,
|
||||||
|
operator_zero_reference=(dict(policy=UPRIGHT_SPLAY_REFERENCE,
|
||||||
|
joints=sorted(UPRIGHT_SPLAY_JOINTS), baseline_command=list(profile.command.baseline_values),
|
||||||
|
independently_measured=False) if upright_splay_zero else None),
|
||||||
|
solver_observations=dict(policy='exact_repeated_pose_least_squares_v1',
|
||||||
|
original_tag_observations=bundle.original_residual_count//8,
|
||||||
|
grouped_tag_observations=len(bundle.residual_weights)//8,
|
||||||
|
within_group_squared_error=bundle.within_group_error),
|
||||||
|
captured_protected_inputs=captured_hashes, solver_protected_inputs=hashes,
|
||||||
|
remount_history=[row for row in records if row.get('kind') == 'raw_tag_remount_checkpoint'],
|
||||||
|
resume_installation_checks=[row.get('installation_verification', {}) for row in records
|
||||||
|
if row.get('kind') == 'raw_resume_verified'],
|
||||||
|
operator_installation_clarification=(json.loads(installation_note.read_text())
|
||||||
|
if installation_note.is_file() else None),
|
||||||
|
model_direction_corrections={name: dict(captured=captured.command.joint_directions[channel],
|
||||||
|
offline=profile.command.joint_directions[channel])
|
||||||
|
for name, channel in profile.command.command_index_by_joint.items()
|
||||||
|
if captured.command.joint_directions[channel] != profile.command.joint_directions[channel]},
|
||||||
|
model_tag_link_corrections={role: dict(captured=captured_links[role], offline=link)
|
||||||
|
for role, link in corrected_links.items() if captured_links.get(role) != link},
|
||||||
|
correction_scope=draft_targets(profile, selected), full_target_generated=False)
|
||||||
|
atomic_write_json(directory/'draft_result.json', state)
|
||||||
|
print(f'离线草稿：{len(tasks)} 个任务，{len(bundle.joints)} 个关节；'
|
||||||
|
f'训练 {len(training)} 帧，独立验证 {len(holdout)} 帧；'
|
||||||
|
f'保留 {len(evidence.missing_units)} 个采集缺项。', flush=True)
|
||||||
|
# Keep every usable steady frame, but release the much larger sweep and
|
||||||
|
# rejected-frame journals before allocating the optimizer's Jacobian.
|
||||||
|
range_frames = fitting_frames(selected, evidence.training)
|
||||||
|
del records, evidence
|
||||||
|
audit_rows = []
|
||||||
|
def progress(row):
|
||||||
|
audit_rows.append(row)
|
||||||
|
atomic_write_json(directory/'draft_training.json', dict(candidates=audit_rows, validation_used=False))
|
||||||
|
try:
|
||||||
|
direction_audit = bundle.audit_root_directions()
|
||||||
|
atomic_write_json(directory/'draft_direction_audit.json', direction_audit)
|
||||||
|
if not direction_audit['passed']:
|
||||||
|
raise ValueError('draft_sdk_to_urdf_direction_conflict')
|
||||||
|
parameters, audit, candidates = fit_draft(bundle, progress)
|
||||||
|
atomic_write_json(directory/'draft_training.json', audit)
|
||||||
|
np.savez_compressed(directory/'draft_candidates.npz',
|
||||||
|
parameters=np.asarray([values for _, values in candidates]),
|
||||||
|
seeds=np.asarray([row['seed'] for row, _ in candidates]))
|
||||||
|
# Freeze the training choice before any validation. Precision cannot
|
||||||
|
# block a draft; structural and source-integrity checks still do.
|
||||||
|
atomic_write_json(directory/'draft_selection.json', audit)
|
||||||
|
validation = evaluate_draft_holdout(bundle, parameters, holdout)
|
||||||
|
atomic_write_json(directory/'draft_holdout.json', validation)
|
||||||
|
mappings, offsets = bundle.output_mappings(parameters), bundle.zero_offsets(parameters)
|
||||||
|
ranges = prepare_raw_motion_ranges(bundle, parameters, range_frames)
|
||||||
|
plan = build_standard_correction_plan(source_urdf=config.source_urdf,
|
||||||
|
source_sha256=config.source_urdf_sha256, zero_offsets_rad=offsets,
|
||||||
|
measured_ranges_output_rad={name: value.output_limits_rad for name, value in ranges.items()},
|
||||||
|
authorized_fields=profile.urdf_authorized_fields, range_evidence=ranges, preserve_source_mimic=True,
|
||||||
|
allow_single_training_cycle=True)
|
||||||
|
payload = dict(format=DRAFT_FORMAT, profile_id=profile.key.profile_id, serial_number=config.serial_number,
|
||||||
|
accuracy_passed=False, publication_allowed=False, baseline_command=list(profile.command.baseline_values),
|
||||||
|
coordinate_convention='q_CAD=q_output+zero_offset; mapping emits q_output',
|
||||||
|
scope='observed_training_travel_and_held_poses; arbitrary_multiaxis_not_validated',
|
||||||
|
correction_scope=state['correction_scope'],
|
||||||
|
retained_joints=sorted(profile.zero.active_joints-set(mappings)),
|
||||||
|
joints={name: dict(zero_offset_rad=offsets[name],
|
||||||
|
zero_status=('operator_defined' if selected.zero.cad_zero_assumptions.get(name)
|
||||||
|
== UPRIGHT_SPLAY_REFERENCE else 'retained_cad'
|
||||||
|
if name in selected.zero.cad_zero_assumptions else 'estimated_draft'),
|
||||||
|
zero_assumption=selected.zero.cad_zero_assumptions.get(name), command_to_rad=_mapping(mapping))
|
||||||
|
for name, mapping in mappings.items()},
|
||||||
|
urdf_correction=serialize_urdf_correction(plan),
|
||||||
|
motion_range_evidence={name: asdict(value) for name, value in ranges.items()},
|
||||||
|
training=audit, holdout=validation, provenance=state)
|
||||||
|
json_path, urdf_path, corrected, maximum = export_draft_artifacts(payload,
|
||||||
|
config.source_urdf, profile, directory, training, standard_loader=standard_loader)
|
||||||
|
from ...core.urdf.linear_drive import UrdfLinearImages
|
||||||
|
linear_validation = evaluate_draft_holdout(UrdfLinearImages(bundle, corrected), parameters, holdout)
|
||||||
|
linear_validation.update(drive='urdf_limits_linear_sdk_u8', parameters_refitted=False)
|
||||||
|
atomic_write_json(directory/'draft_urdf_linear_holdout.json', linear_validation)
|
||||||
|
if digests != {str(path): file_digest(path) for path in files}:
|
||||||
|
raise ValueError('draft_source_capture_changed_during_fit')
|
||||||
|
if hint_audit is not None and hint_audit['input_sha256'] != {
|
||||||
|
path: file_digest(Path(path)) for path in hint_audit['input_sha256']}:
|
||||||
|
raise ValueError('draft_initialization_changed_during_fit')
|
||||||
|
state.update(generated=True, source_unchanged=True, urdf=str(urdf_path),
|
||||||
|
full_target_generated=state['correction_scope']['full_target_covered'],
|
||||||
|
urdf_sha256=file_digest(urdf_path), calibration_json=str(json_path),
|
||||||
|
corrected_joints=sorted(plan.patches.joints), zero_offsets_rad=offsets,
|
||||||
|
retained_zero_assumptions=dict(selected.zero.cad_zero_assumptions),
|
||||||
|
final_file_fk_maximum_difference=maximum, standard_loader_passed=True,
|
||||||
|
independent_holdout_passed=validation.get('passed', False))
|
||||||
|
state['urdf_linear_holdout_passed'] = linear_validation.get('passed', False)
|
||||||
|
atomic_write_json(directory/'draft_result.json', state)
|
||||||
|
print(f'已生成修正草稿：{urdf_path}；零位及行程见 draft_calibration.json。精度尚未认证。', flush=True)
|
||||||
|
scope = state['correction_scope']
|
||||||
|
print(f"零位 {len(scope['estimated_zero_joints'])}/{len(scope['zero_target_joints'])}、"
|
||||||
|
f"另有用户定义竖直零位 {len(scope['defined_zero_joints'])} 个；"
|
||||||
|
f"运动范围 {len(scope['estimated_range_joints'])}/{len(scope['range_target_joints'])}；"
|
||||||
|
f"五指末端零位保留 CAD。全手参数已生成：{state['full_target_generated']}；"
|
||||||
|
f"独立验证通过：{state['independent_holdout_passed']}；草稿不允许正式发布。", flush=True)
|
||||||
|
return state
|
||||||
|
except Exception as error:
|
||||||
|
state['reason'] = str(error)
|
||||||
|
atomic_write_json(directory/'draft_result.json', state)
|
||||||
|
raise
|
||||||
+6
-5
@@ -29,7 +29,8 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
|
|||||||
from .finalization import CorrectedArtifact
|
from .finalization import CorrectedArtifact
|
||||||
directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve()
|
directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve()
|
||||||
directory.mkdir(parents=True, exist_ok=True)
|
directory.mkdir(parents=True, exist_ok=True)
|
||||||
state = dict(protocol='raw_joint_images_v1', raw_capture_complete=False,
|
state = dict(protocol=next((r['capture_plan']['observation_protocol']['version']
|
||||||
|
for r in records if r.get('kind') == 'session_start'), 'unknown'), raw_capture_complete=False,
|
||||||
fit_complete=False, accuracy_passed=False, publication_allowed=False)
|
fit_complete=False, accuracy_passed=False, publication_allowed=False)
|
||||||
def save():
|
def save():
|
||||||
atomic_write_json(directory/'raw_joint_result.json', state)
|
atomic_write_json(directory/'raw_joint_result.json', state)
|
||||||
@@ -48,8 +49,9 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
|
|||||||
raise ValueError('raw_joint_source_urdf_changed')
|
raise ValueError('raw_joint_source_urdf_changed')
|
||||||
from ..motion_provenance import validate_source_geometry
|
from ..motion_provenance import validate_source_geometry
|
||||||
validate_source_geometry(profile, source, records)
|
validate_source_geometry(profile, source, records)
|
||||||
evidence = partition_raw_capture(profile, records, require_complete=False)
|
evidence = partition_raw_capture(profile, records, require_complete=False, reassess_quality=True)
|
||||||
state['missing_units'] = evidence.missing_units
|
state['missing_units'] = evidence.missing_units
|
||||||
|
state['acquisition_reassessment'] = evidence.reassessed_units
|
||||||
if evidence.missing_units:
|
if evidence.missing_units:
|
||||||
raise ValueError('raw_joint_capture_incomplete:see_missing_units')
|
raise ValueError('raw_joint_capture_incomplete:see_missing_units')
|
||||||
state.update(raw_capture_complete=True, capture_sha256=evidence.sha256)
|
state.update(raw_capture_complete=True, capture_sha256=evidence.sha256)
|
||||||
@@ -63,10 +65,9 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
|
|||||||
matrices[row['view']] = matrix
|
matrices[row['view']] = matrix
|
||||||
reference = next(r for r in records if r.get('kind') == 'fixed_base_reference_locked')
|
reference = next(r for r in records if r.get('kind') == 'fixed_base_reference_locked')
|
||||||
from ..raw_resume import RawCheckpoint
|
from ..raw_resume import RawCheckpoint
|
||||||
from ...core.domain.capture_plan import CapturePlan
|
|
||||||
for resumed in (r for r in records if r.get('kind') == 'raw_resume_verified'):
|
for resumed in (r for r in records if r.get('kind') == 'raw_resume_verified'):
|
||||||
header = resumed['source_header']
|
header = resumed['source_header']
|
||||||
if (header.get('capture_plan') != CapturePlan.from_profile(profile).as_dict()
|
if (header.get('capture_plan') != evidence.capture_plan
|
||||||
or header.get('serial_number') != serial_number
|
or header.get('serial_number') != serial_number
|
||||||
or any(header.get(key) != value for key, value in hashes.items())):
|
or any(header.get(key) != value for key, value in hashes.items())):
|
||||||
raise ValueError('raw_joint_imported_provenance_changed')
|
raise ValueError('raw_joint_imported_provenance_changed')
|
||||||
@@ -159,7 +160,7 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
|
|||||||
except Exception as error:
|
except Exception as error:
|
||||||
state.update(reason=str(error), publication_allowed=False,
|
state.update(reason=str(error), publication_allowed=False,
|
||||||
automatic_geometry_recapture=False,
|
automatic_geometry_recapture=False,
|
||||||
next_action=('recapture_only_missing_units' if state.get('missing_units') else
|
next_action=('inspect_saved_missing_units_without_automatic_motion' if state.get('missing_units') else
|
||||||
'inspect_training_candidate_and_holdout_reports_before_selecting_new_observations'))
|
'inspect_training_candidate_and_holdout_reports_before_selecting_new_observations'))
|
||||||
save()
|
save()
|
||||||
raise
|
raise
|
||||||
|
|||||||
+66
-15
@@ -2,25 +2,83 @@
|
|||||||
from dataclasses import asdict
|
from dataclasses import asdict
|
||||||
|
|
||||||
from ...core.domain.capture_plan import CapturePlan, task_for_joint, evidence_digest
|
from ...core.domain.capture_plan import CapturePlan, task_for_joint, evidence_digest
|
||||||
|
from ...core.domain.motion_path import task_segment
|
||||||
from ...core.urdf.limits import MotionRangeEvidence
|
from ...core.urdf.limits import MotionRangeEvidence
|
||||||
from ...core.urdf.plan import build_standard_correction_plan
|
from ...core.urdf.plan import build_standard_correction_plan
|
||||||
from .serializers.measured_v3 import REPORT_FORMAT, ZERO_CONVENTION
|
from .serializers.measured_v3 import REPORT_FORMAT, ZERO_CONVENTION
|
||||||
from .serializers.unified_v1 import _mapping
|
from .serializers.unified_v1 import _mapping
|
||||||
from .urdf_from_json import serialize_urdf_correction
|
from .urdf_from_json import serialize_urdf_correction
|
||||||
|
from ..raw_evidence import fitting_frames
|
||||||
|
from ...core.fitting.command_support import TURNAROUND_POLICY, validate_turnaround_mapping
|
||||||
|
|
||||||
|
|
||||||
|
def joint_training_rows(profile, training, name):
|
||||||
|
"""Only visible, active training observations support a joint's range."""
|
||||||
|
task = task_for_joint(profile, name)
|
||||||
|
spec = profile.measurement.measurements[name]
|
||||||
|
return [row for row in training if row['task_name'] == task.key
|
||||||
|
and row.get('cycle') in (0, 1) and row['view'] == spec.view
|
||||||
|
and spec.child_role in row.get('tags', {})
|
||||||
|
and (not task.segments or name in task_segment(task, row.get('segment_key', '')).joints)]
|
||||||
|
|
||||||
|
|
||||||
|
def prepare_raw_motion_ranges(bundle, parameters, training):
|
||||||
|
"""Training-supported travel, shared by drafts and precision releases."""
|
||||||
|
profile = bundle.profile
|
||||||
|
training = fitting_frames(profile, training)
|
||||||
|
mappings, offsets = bundle.output_mappings(parameters), bundle.zero_offsets(parameters)
|
||||||
|
ranges = {}
|
||||||
|
for name, mapping in mappings.items():
|
||||||
|
rows = joint_training_rows(profile, training, name)
|
||||||
|
baseline = profile.command.baseline_values[mapping.motor_index]
|
||||||
|
baseline_rows = [row for row in rows if row['command_vector'][mapping.motor_index] == baseline]
|
||||||
|
if not baseline_rows:
|
||||||
|
raise ValueError('raw_joint_baseline_support_missing:' + name)
|
||||||
|
policy = profile.urdf_limit_policies[name]
|
||||||
|
original = bundle.model.joints[bundle.cad_names[name]]
|
||||||
|
ids = tuple(sorted(f"{row['view']}:{row['image_stamp_ns']}" for row in rows))
|
||||||
|
baseline_ids = tuple(sorted(f"{r['view']}:{r['image_stamp_ns']}" for r in baseline_rows))
|
||||||
|
bounds = min(0., mapping.bounds_rad[0]), max(0., mapping.bounds_rad[1])
|
||||||
|
cycles = tuple(sorted({row['cycle'] for row in rows}))
|
||||||
|
support = None
|
||||||
|
if hasattr(bundle, 'turnaround_constraints'):
|
||||||
|
constraints = [{key: value for key, value in row.items() if key != 'column'}
|
||||||
|
for row in bundle.turnaround_constraints.get(name, ())]
|
||||||
|
endpoints = []
|
||||||
|
for command in (mapping.knots[0], mapping.knots[-1]):
|
||||||
|
observed = [r for r in rows if r['command_vector'][mapping.motor_index] == command]
|
||||||
|
if not observed:
|
||||||
|
raise ValueError('raw_joint_endpoint_support_missing:' + name + ':' + str(command))
|
||||||
|
for direction, label in enumerate(('increasing', 'decreasing')):
|
||||||
|
if (command != baseline and not any(r['command_direction_by_index'][mapping.motor_index] == label
|
||||||
|
for r in observed) and not any(c['command'] == command and c['recipient'] == direction
|
||||||
|
for c in constraints)):
|
||||||
|
raise ValueError('raw_joint_directional_endpoint_support_missing:' + name + ':' + label)
|
||||||
|
endpoints.append(dict(command=command, samples=[dict(
|
||||||
|
id=f"{r['view']}:{r['image_stamp_ns']}", cycle=r['cycle'],
|
||||||
|
direction=r['command_direction_by_index'][mapping.motor_index]) for r in observed]))
|
||||||
|
support = dict(policy=TURNAROUND_POLICY, constraints=constraints,
|
||||||
|
endpoints=endpoints, independently_measured_outgoing_endpoint=False if constraints else None)
|
||||||
|
validate_turnaround_mapping(mapping, support)
|
||||||
|
ranges[name] = MotionRangeEvidence(name, name, True, (original.lower, original.upper),
|
||||||
|
bounds, None, mapping.bounds_rad, offsets[name], baseline, cycles, ids, baseline_ids,
|
||||||
|
policy.source_limits, policy.evidence_source, policy.independent_safety_limits_cad_rad,
|
||||||
|
policy.safety_evidence_source, cycles, ids, 'steady_command', support)
|
||||||
|
return ranges
|
||||||
|
|
||||||
|
|
||||||
def prepare_raw_report(bundle, fit, evidence, validation, *, serial_number, protected_inputs):
|
def prepare_raw_report(bundle, fit, evidence, validation, *, serial_number, protected_inputs):
|
||||||
profile = bundle.profile
|
profile = bundle.profile
|
||||||
candidate = fit.candidates[fit.representative]
|
candidate = fit.candidates[fit.representative]
|
||||||
mappings, offsets = bundle.output_mappings(candidate.parameters), bundle.zero_offsets(candidate.parameters)
|
mappings, offsets = bundle.output_mappings(candidate.parameters), bundle.zero_offsets(candidate.parameters)
|
||||||
ranges, references = {}, {}
|
ranges = prepare_raw_motion_ranges(bundle, candidate.parameters, evidence.training)
|
||||||
|
references = {}
|
||||||
|
training_rows = fitting_frames(profile, evidence.training)
|
||||||
training_digest = evidence_digest(evidence.training)
|
training_digest = evidence_digest(evidence.training)
|
||||||
candidate_digest = evidence_digest([dict(seed=list(c.seed), parameters=c.parameters.tolist(),
|
candidate_digest = evidence_digest([dict(seed=list(c.seed), parameters=c.parameters.tolist(),
|
||||||
covariance=c.covariance.tolist()) for c in fit.candidates])
|
covariance=c.covariance.tolist()) for c in fit.candidates])
|
||||||
for name, mapping in mappings.items():
|
for name, mapping in mappings.items():
|
||||||
task = task_for_joint(profile, name)
|
rows = joint_training_rows(profile, training_rows, name)
|
||||||
rows = [row for row in evidence.training if row['task_name'] == task.key
|
|
||||||
and row['sample_phase'] == 'steady' and row.get('accepted_sample_window_open')]
|
|
||||||
baseline = profile.command.baseline_values[mapping.motor_index]
|
baseline = profile.command.baseline_values[mapping.motor_index]
|
||||||
baseline_rows = [row for row in rows if row['command_vector'][mapping.motor_index] == baseline]
|
baseline_rows = [row for row in rows if row['command_vector'][mapping.motor_index] == baseline]
|
||||||
if not baseline_rows:
|
if not baseline_rows:
|
||||||
@@ -29,15 +87,6 @@ def prepare_raw_report(bundle, fit, evidence, validation, *, serial_number, prot
|
|||||||
baseline_command=baseline, training_sha256=training_digest, candidate_set_sha256=candidate_digest,
|
baseline_command=baseline, training_sha256=training_digest, candidate_set_sha256=candidate_digest,
|
||||||
physical_zero_source='joint_urdf_image_graph',
|
physical_zero_source='joint_urdf_image_graph',
|
||||||
samples=[dict(view=r['view'], image_stamp_ns=r['image_stamp_ns']) for r in baseline_rows])
|
samples=[dict(view=r['view'], image_stamp_ns=r['image_stamp_ns']) for r in baseline_rows])
|
||||||
policy = profile.urdf_limit_policies[name]
|
|
||||||
original = bundle.model.joints[bundle.cad_names[name]]
|
|
||||||
ids = tuple(sorted(f"{row['view']}:{row['image_stamp_ns']}" for row in rows))
|
|
||||||
baseline_ids = tuple(sorted(f"{r['view']}:{r['image_stamp_ns']}" for r in baseline_rows))
|
|
||||||
bounds = min(0., mapping.bounds_rad[0]), max(0., mapping.bounds_rad[1])
|
|
||||||
ranges[name] = MotionRangeEvidence(name, name, True, (original.lower, original.upper),
|
|
||||||
bounds, None, mapping.bounds_rad, offsets[name], baseline, (0, 1), ids, baseline_ids,
|
|
||||||
policy.source_limits, policy.evidence_source, policy.independent_safety_limits_cad_rad,
|
|
||||||
policy.safety_evidence_source, (0, 1), ids, 'steady_command')
|
|
||||||
plan = build_standard_correction_plan(source_urdf=bundle.model.source,
|
plan = build_standard_correction_plan(source_urdf=bundle.model.source,
|
||||||
source_sha256=protected_inputs['source_urdf_sha256'], zero_offsets_rad=offsets,
|
source_sha256=protected_inputs['source_urdf_sha256'], zero_offsets_rad=offsets,
|
||||||
measured_ranges_output_rad={name: value.output_limits_rad for name, value in ranges.items()},
|
measured_ranges_output_rad={name: value.output_limits_rad for name, value in ranges.items()},
|
||||||
@@ -66,10 +115,12 @@ def prepare_raw_report(bundle, fit, evidence, validation, *, serial_number, prot
|
|||||||
feedback_by_index=profile.command.feedback_by_index,
|
feedback_by_index=profile.command.feedback_by_index,
|
||||||
feedback_name_aliases=dict(profile.command.feedback_name_aliases),
|
feedback_name_aliases=dict(profile.command.feedback_name_aliases),
|
||||||
protected_inputs=dict(protected_inputs), joints=joints,
|
protected_inputs=dict(protected_inputs), joints=joints,
|
||||||
capture_plan=CapturePlan.from_profile(profile).as_dict(),
|
capture_plan=evidence.capture_plan,
|
||||||
urdf_correction=serialize_urdf_correction(plan),
|
urdf_correction=serialize_urdf_correction(plan),
|
||||||
motion_range_evidence={name: asdict(value) for name, value in ranges.items()},
|
motion_range_evidence={name: asdict(value) for name, value in ranges.items()},
|
||||||
raw_joint_audit=dict(protocol='raw_joint_images_v1', capture_sha256=evidence.sha256,
|
raw_joint_audit=dict(protocol=evidence.capture_plan['observation_protocol']['version'], capture_sha256=evidence.sha256,
|
||||||
|
command_support={name: value.command_support for name, value in ranges.items()},
|
||||||
|
acquisition_reassessment=evidence.reassessed_units,
|
||||||
training_sha256=training_digest, candidate_set_sha256=candidate_digest,
|
training_sha256=training_digest, candidate_set_sha256=candidate_digest,
|
||||||
candidate_uncertainty=fit.uncertainty, candidate_count=len(fit.candidates),
|
candidate_uncertainty=fit.uncertainty, candidate_count=len(fit.candidates),
|
||||||
training_selection=fit.training_selection,
|
training_selection=fit.training_selection,
|
||||||
|
|||||||
@@ -12,6 +12,12 @@ def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool
|
|||||||
from ..runner_support import create_session_directory, protected_inputs
|
from ..runner_support import create_session_directory, protected_inputs
|
||||||
records = load_capture(raw_path)
|
records = load_capture(raw_path)
|
||||||
profile = config.calibration_contract.typed_profile
|
profile = config.calibration_contract.typed_profile
|
||||||
|
header = next((r for r in records if r.get('kind') == 'session_start'), {})
|
||||||
|
if header.get('capture_plan', {}).get('observation_protocol', {}).get('version') == 'raw_joint_images_v2':
|
||||||
|
from ..raw_completion import verify_capture_manifest
|
||||||
|
manifest = verify_capture_manifest(raw_path)
|
||||||
|
if manifest['capture_plan'] != header['capture_plan'] or manifest['serial_number'] != config.serial_number:
|
||||||
|
raise ValueError('raw_capture_manifest_identity_changed')
|
||||||
hashes = protected_inputs(config)
|
hashes = protected_inputs(config)
|
||||||
reference = next((row for row in records if row.get("kind") == "fixed_base_reference_locked"), {})
|
reference = next((row for row in records if row.get("kind") == "fixed_base_reference_locked"), {})
|
||||||
if "intrinsics_sha256" in reference.get("protected_hashes", {}):
|
if "intrinsics_sha256" in reference.get("protected_hashes", {}):
|
||||||
@@ -22,6 +28,12 @@ def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool
|
|||||||
else:
|
else:
|
||||||
output = output.expanduser().resolve()
|
output = output.expanduser().resolve()
|
||||||
output.mkdir(parents=True, exist_ok=False)
|
output.mkdir(parents=True, exist_ok=False)
|
||||||
|
from ..raw_completion import implementation_digest
|
||||||
|
from ...storage import atomic_write_json
|
||||||
|
atomic_write_json(output / 'offline_run.json', dict(
|
||||||
|
input_raw=str(raw_path.resolve()), input_sha256=hashes['raw_samples_sha256'],
|
||||||
|
capture_plan=header.get('capture_plan'), implementation_sha256=implementation_digest(),
|
||||||
|
protected_inputs=hashes, commands_enabled=False, publish_requested=publish))
|
||||||
from ...core.domain.capture_plan import STAGED
|
from ...core.domain.capture_plan import STAGED
|
||||||
if profile.quality.training_policy == STAGED:
|
if profile.quality.training_policy == STAGED:
|
||||||
import shutil
|
import shutil
|
||||||
|
|||||||
+11
-1
@@ -117,7 +117,8 @@ def from_report(report):
|
|||||||
if raw_audit is not None:
|
if raw_audit is not None:
|
||||||
from ....core.domain.capture_plan import RAW_JOINT
|
from ....core.domain.capture_plan import RAW_JOINT
|
||||||
if (report.get('capture_plan', {}).get('policy') != RAW_JOINT
|
if (report.get('capture_plan', {}).get('policy') != RAW_JOINT
|
||||||
or raw_audit.get('protocol') != 'raw_joint_images_v1'
|
or raw_audit.get('protocol') not in {'raw_joint_images_v1', 'raw_joint_images_v2'}
|
||||||
|
or raw_audit.get('protocol') != report['capture_plan'].get('observation_protocol', {}).get('version')
|
||||||
or raw_audit.get('candidate_uncertainty', {}).get('passed') is not True
|
or raw_audit.get('candidate_uncertainty', {}).get('passed') is not True
|
||||||
or raw_audit.get('validation_parameters_updated') is not False):
|
or raw_audit.get('validation_parameters_updated') is not False):
|
||||||
raise ValueError('raw_joint_report_not_certified')
|
raise ValueError('raw_joint_report_not_certified')
|
||||||
@@ -126,6 +127,15 @@ def from_report(report):
|
|||||||
for key in ('capture_sha256', 'training_sha256', 'candidate_set_sha256')):
|
for key in ('capture_sha256', 'training_sha256', 'candidate_set_sha256')):
|
||||||
raise ValueError('raw_joint_report_digest_invalid')
|
raise ValueError('raw_joint_report_digest_invalid')
|
||||||
for name, row in report['joints'].items():
|
for name, row in report['joints'].items():
|
||||||
|
if 'command_support' in raw_audit:
|
||||||
|
from ....core.domain.result import JointMapping
|
||||||
|
from ....core.fitting.command_support import validate_turnaround_mapping
|
||||||
|
table = row['command_to_rad']
|
||||||
|
command = JointMapping(name, table['channel_index'], table['input_domain'],
|
||||||
|
*(tuple(table[key]) for key in ('knots', 'angle_rad', 'increasing_rad', 'decreasing_rad')))
|
||||||
|
if name not in raw_audit['command_support']:
|
||||||
|
raise ValueError('raw_joint_command_support_missing:' + name)
|
||||||
|
validate_turnaround_mapping(command, raw_audit['command_support'][name])
|
||||||
reference = row.get('zero_reference', {})
|
reference = row.get('zero_reference', {})
|
||||||
if (reference.get('kind') != 'raw_joint_training_baseline'
|
if (reference.get('kind') != 'raw_joint_training_baseline'
|
||||||
or reference.get('training_sha256') != raw_audit.get('training_sha256')
|
or reference.get('training_sha256') != raw_audit.get('training_sha256')
|
||||||
|
|||||||
@@ -41,6 +41,7 @@ class CaptureFrame:
|
|||||||
observed_roles: frozenset[str] = frozenset()
|
observed_roles: frozenset[str] = frozenset()
|
||||||
timing: CaptureTiming | None = None
|
timing: CaptureTiming | None = None
|
||||||
command_skew_ns: int | None = None
|
command_skew_ns: int | None = None
|
||||||
|
detection_quality: dict | None = None
|
||||||
|
|
||||||
|
|
||||||
class _FramePoseEvidence:
|
class _FramePoseEvidence:
|
||||||
@@ -331,9 +332,53 @@ class ObservationCapture:
|
|||||||
"camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]",
|
"camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]",
|
||||||
"input_is_rectified": True, "roles": observations}
|
"input_is_rectified": True, "roles": observations}
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _observation_record(frame, motion, evidence, allow_recording, raw_joint, command_unit):
|
||||||
|
return {"kind": "raw_observation_frame" if raw_joint else "diagnostic_observation_frame", "view": frame.view,
|
||||||
|
"image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key,
|
||||||
|
"cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt,
|
||||||
|
**segment_metadata(motion.segment_key),
|
||||||
|
"sample_phase": motion.phase, "command_unit": command_unit,
|
||||||
|
**({"zero_joints": list(motion.zero_joints)} if motion.phase == "joint_zero" else {}),
|
||||||
|
**({"steady_index": motion.steady_index,
|
||||||
|
"steady_target": None if motion.command_index is None else motion.target[motion.command_index],
|
||||||
|
"minimum_hold_seconds": motion.minimum_hold_seconds,
|
||||||
|
"target_command_vector": list(motion.target),
|
||||||
|
"accepted_sample_window_open": bool(allow_recording)} if motion.phase == "steady" else {}),
|
||||||
|
"command_vector": None if frame.command is None else list(frame.command),
|
||||||
|
"feedback_vector": None if frame.feedback is None else list(frame.feedback),
|
||||||
|
"state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6,
|
||||||
|
"command_image_skew_ns": frame.command_skew_ns,
|
||||||
|
"command_direction_by_index": list(frame.command_directions),
|
||||||
|
**({"timing": asdict(frame.timing)} if frame.timing is not None else {}),
|
||||||
|
"camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True,
|
||||||
|
"tags": evidence,
|
||||||
|
**({"observation_protocol": "raw_joint_images_v2"} if raw_joint else {}), "is_accepted_joint_sample": False}
|
||||||
|
|
||||||
|
def _consume_raw(self, frame, motion, allow_recording):
|
||||||
|
# Only the fixed reference runs through pose tracking. Moving-tag
|
||||||
|
# evidence is independent of any PnP branch or online joint model.
|
||||||
|
with self._metadata_lock:
|
||||||
|
if frame.stamp_ns <= self._latest_stamp_by_view.get(frame.view, 0):
|
||||||
|
return (), None
|
||||||
|
events, movement = self.consume(frame, fixed_reference_only=True)
|
||||||
|
if motion is None or motion.phase not in {"sweep", "steady"}:
|
||||||
|
return events, movement
|
||||||
|
view = next(v for v in self.profile.vision.views if v.name == frame.view)
|
||||||
|
evidence = {tag.role: dict(tag_role=tag.role, tag_id=tag.tag_id,
|
||||||
|
tag_size_m=float(tag.size_m), corners_xy=np.asarray(frame.corners[tag.role]).tolist(),
|
||||||
|
image_stamp_ns=frame.stamp_ns, pose_source="raw_corners",
|
||||||
|
detection_quality=(frame.detection_quality or {}).get(tag.role, {}))
|
||||||
|
for tag in view.tags if tag.role in frame.corners}
|
||||||
|
record = self._observation_record(frame, motion, evidence, allow_recording,
|
||||||
|
raw_joint=True, command_unit=self.profile.command.unit)
|
||||||
|
return (*events, record), movement
|
||||||
|
|
||||||
def consume(self, frame: CaptureFrame, motion=None, *, collect_installations=False,
|
def consume(self, frame: CaptureFrame, motion=None, *, collect_installations=False,
|
||||||
diagnostic=False, allow_recording=True, accepted_joints: frozenset[str] | None = None,
|
diagnostic=False, allow_recording=True, accepted_joints: frozenset[str] | None = None,
|
||||||
fixed_reference_only=False, raw_joint=False):
|
fixed_reference_only=False, raw_joint=False):
|
||||||
|
if raw_joint:
|
||||||
|
return self._consume_raw(frame, motion, allow_recording)
|
||||||
if fixed_reference_only and (motion is not None or collect_installations):
|
if fixed_reference_only and (motion is not None or collect_installations):
|
||||||
raise ValueError("fixed reference monitoring cannot acquire motion or installations")
|
raise ValueError("fixed reference monitoring cannot acquire motion or installations")
|
||||||
if frame.view not in self.reference_lock.fixed_tag_by_view:
|
if frame.view not in self.reference_lock.fixed_tag_by_view:
|
||||||
@@ -347,7 +392,7 @@ class ObservationCapture:
|
|||||||
image_reference = self._scheduled_image_reference(frame.view, models)
|
image_reference = self._scheduled_image_reference(frame.view, models)
|
||||||
view = next(v for v in self.profile.vision.views if v.name == frame.view)
|
view = next(v for v in self.profile.vision.views if v.name == frame.view)
|
||||||
tags = tuple(tag for tag in view.tags if not fixed_reference_only or tag.fixed_reference)
|
tags = tuple(tag for tag in view.tags if not fixed_reference_only or tag.fixed_reference)
|
||||||
if not raw_joint and self.profile.acquisition.motion_model_scope == "task" and (motion is not None or self._motion_models):
|
if self.profile.acquisition.motion_model_scope == "task" and (motion is not None or self._motion_models):
|
||||||
relevant = self._observed_roles(motion)
|
relevant = self._observed_roles(motion)
|
||||||
if motion is not None:
|
if motion is not None:
|
||||||
from .observer_reference import observer_roles
|
from .observer_reference import observer_roles
|
||||||
@@ -479,30 +524,13 @@ class ObservationCapture:
|
|||||||
"state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6,
|
"state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6,
|
||||||
"camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True,
|
"camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True,
|
||||||
"tags": evidence, "is_accepted_joint_sample": False})
|
"tags": evidence, "is_accepted_joint_sample": False})
|
||||||
diagnostic_raw = ((diagnostic or raw_joint) and motion is not None and (
|
diagnostic_raw = (diagnostic and motion is not None and (
|
||||||
motion.phase == "steady" or (motion.phase in {"sweep", "joint_zero"} and allow_recording)))
|
motion.phase == "steady" or (motion.phase in {"sweep", "joint_zero"} and allow_recording)))
|
||||||
if diagnostic_raw:
|
if diagnostic_raw:
|
||||||
# Raw candidate capture has its own record type. It cannot be
|
# Raw candidate capture has its own record type. It cannot be
|
||||||
# mistaken for an accepted joint angle or enter formal fitting.
|
# mistaken for an accepted joint angle or enter formal fitting.
|
||||||
events.append({"kind": "raw_observation_frame" if raw_joint else "diagnostic_observation_frame", "view": frame.view,
|
events.append(self._observation_record(frame, motion, evidence,
|
||||||
"image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key,
|
allow_recording, raw_joint=False, command_unit=self.profile.command.unit))
|
||||||
"cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt,
|
|
||||||
**segment_metadata(motion.segment_key),
|
|
||||||
"sample_phase": motion.phase, "command_unit": self.profile.command.unit,
|
|
||||||
**({"zero_joints": list(motion.zero_joints)} if motion.phase == "joint_zero" else {}),
|
|
||||||
**({"steady_index": motion.steady_index,
|
|
||||||
"steady_target": None if motion.command_index is None else motion.target[motion.command_index],
|
|
||||||
"minimum_hold_seconds": motion.minimum_hold_seconds,
|
|
||||||
"target_command_vector": list(motion.target),
|
|
||||||
"accepted_sample_window_open": bool(allow_recording)} if motion.phase == "steady" else {}),
|
|
||||||
"command_vector": None if frame.command is None else list(frame.command),
|
|
||||||
"feedback_vector": None if frame.feedback is None else list(frame.feedback),
|
|
||||||
"state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6,
|
|
||||||
"command_image_skew_ns": frame.command_skew_ns,
|
|
||||||
"command_direction_by_index": list(frame.command_directions),
|
|
||||||
**({"timing": asdict(frame.timing)} if frame.timing is not None else {}),
|
|
||||||
"camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True,
|
|
||||||
"tags": evidence, "is_accepted_joint_sample": False})
|
|
||||||
if movement is not None or motion is None or not motion.recording or not allow_recording:
|
if movement is not None or motion is None or not motion.recording or not allow_recording:
|
||||||
return tuple(events), movement
|
return tuple(events), movement
|
||||||
task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key)
|
task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key)
|
||||||
@@ -515,8 +543,6 @@ class ObservationCapture:
|
|||||||
streams = [item for item in streams if item[1] in motion.zero_joints]
|
streams = [item for item in streams if item[1] in motion.zero_joints]
|
||||||
elif motion.observed_joints:
|
elif motion.observed_joints:
|
||||||
streams = [item for item in streams if item[1] in motion.observed_joints]
|
streams = [item for item in streams if item[1] in motion.observed_joints]
|
||||||
if raw_joint:
|
|
||||||
return tuple(events), movement
|
|
||||||
if diagnostic:
|
if diagnostic:
|
||||||
streams = [item for item in streams if item[1] in (accepted_joints or ())]
|
streams = [item for item in streams if item[1] in (accepted_joints or ())]
|
||||||
if not streams or (frame.feedback is None and not self.profile.vision_motion) or frame.command is None:
|
if not streams or (frame.feedback is None and not self.profile.vision_motion) or frame.command is None:
|
||||||
|
|||||||
@@ -46,7 +46,7 @@ from ..core.domain.capture_plan import (CapturePlan, ADAPTIVE, STAGED,
|
|||||||
TASKWISE_STAGED, STAGED_POLICIES, raw_joint_capture)
|
TASKWISE_STAGED, STAGED_POLICIES, raw_joint_capture)
|
||||||
|
|
||||||
|
|
||||||
_TERMINAL_STATES = frozenset({"PASSED", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"})
|
_TERMINAL_STATES = frozenset({"PASSED", "CAPTURE_COMPLETE", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"})
|
||||||
|
|
||||||
|
|
||||||
class CalibrationCoordinator:
|
class CalibrationCoordinator:
|
||||||
@@ -86,7 +86,7 @@ class CalibrationCoordinator:
|
|||||||
self.training_worker = TrainingWorker()
|
self.training_worker = TrainingWorker()
|
||||||
self.safety = SafetyPolicy(profile)
|
self.safety = SafetyPolicy(profile)
|
||||||
from .stall_retry import StallRetry
|
from .stall_retry import StallRetry
|
||||||
self.stall_retry = StallRetry() if profile.key.model.upper() == 'O30' else None
|
self.stall_retry = StallRetry(maximum_failures=2 if self.raw_joint else 3) if profile.key.model.upper() == 'O30' else None
|
||||||
self.reference_lock = self._new_reference_lock()
|
self.reference_lock = self._new_reference_lock()
|
||||||
self.trackers = parameters.new_trackers(self.required_observation_views)
|
self.trackers = parameters.new_trackers(self.required_observation_views)
|
||||||
self.capture = ObservationCapture(profile, reference_lock=self.reference_lock,
|
self.capture = ObservationCapture(profile, reference_lock=self.reference_lock,
|
||||||
@@ -119,6 +119,8 @@ class CalibrationCoordinator:
|
|||||||
self._observation_epoch = 0
|
self._observation_epoch = 0
|
||||||
self._pause_effects_applied = False
|
self._pause_effects_applied = False
|
||||||
self.segment = None
|
self.segment = None
|
||||||
|
self._direction_progress = None
|
||||||
|
self._raw_capture_closed = False
|
||||||
self._settled_pose = None
|
self._settled_pose = None
|
||||||
self._segment_number = 0
|
self._segment_number = 0
|
||||||
self._motion = None
|
self._motion = None
|
||||||
@@ -291,6 +293,8 @@ class CalibrationCoordinator:
|
|||||||
with self.step_data_lock:
|
with self.step_data_lock:
|
||||||
if self.state in {"FAILED", "ABORTED"}:
|
if self.state in {"FAILED", "ABORTED"}:
|
||||||
return ServiceResult(False, self.reason)
|
return ServiceResult(False, self.reason)
|
||||||
|
if self.state == "CAPTURE_COMPLETE":
|
||||||
|
return ServiceResult(False, "raw_capture_already_complete")
|
||||||
if self.state == "DIAGNOSTIC_COMPLETE":
|
if self.state == "DIAGNOSTIC_COMPLETE":
|
||||||
return ServiceResult(False, "diagnostic_capture_already_complete")
|
return ServiceResult(False, "diagnostic_capture_already_complete")
|
||||||
if self.state == "PASSED":
|
if self.state == "PASSED":
|
||||||
@@ -302,6 +306,8 @@ class CalibrationCoordinator:
|
|||||||
|
|
||||||
def receive_feedback(self, names, positions, stamp_ns: int, *, channel_stamps_ns=()) -> StateSample | None:
|
def receive_feedback(self, names, positions, stamp_ns: int, *, channel_stamps_ns=()) -> StateSample | None:
|
||||||
with self.step_data_lock:
|
with self.step_data_lock:
|
||||||
|
if self._raw_capture_closed:
|
||||||
|
return None
|
||||||
return self._accept_feedback(names, positions, stamp_ns, channel_stamps_ns=channel_stamps_ns)
|
return self._accept_feedback(names, positions, stamp_ns, channel_stamps_ns=channel_stamps_ns)
|
||||||
|
|
||||||
def receive_camera_info(self, view: str, message: CameraModelInput) -> None:
|
def receive_camera_info(self, view: str, message: CameraModelInput) -> None:
|
||||||
@@ -433,12 +439,15 @@ class CalibrationCoordinator:
|
|||||||
frame = CaptureFrame(view, stamp, matrix, corners,
|
frame = CaptureFrame(view, stamp, matrix, corners,
|
||||||
None if matched is None else matched[0], command, 0 if matched is None else matched[1],
|
None if matched is None else matched[0], command, 0 if matched is None else matched[1],
|
||||||
next((d for t, d in reversed(direction_history) if t <= stamp), ()), observed_roles, timing,
|
next((d for t, d in reversed(direction_history) if t <= stamp), ()), observed_roles, timing,
|
||||||
None if command_sample is None else stamp-command_sample.stamp_ns)
|
None if command_sample is None else stamp-command_sample.stamp_ns,
|
||||||
|
{tag.role: dict(hamming=d.hamming, decision_margin=d.decision_margin,
|
||||||
|
border_quality=d.border_quality) for tag in self._view(view).tags
|
||||||
|
for d in observation.detections if d.tag_id == tag.tag_id})
|
||||||
record_motion = motion
|
record_motion = motion
|
||||||
if motion is not None and motion.phase in {"steady", "joint_zero"} and (steady_after is None or stamp < steady_after):
|
if motion is not None and motion.phase in {"steady", "joint_zero"} and (steady_after is None or stamp < steady_after):
|
||||||
record_motion = None
|
record_motion = None
|
||||||
from .image_capture import image_observation_record
|
from .image_capture import image_observation_record
|
||||||
raw_image = image_observation_record(self.profile, frame, record_motion)
|
raw_image = None if self.raw_joint else image_observation_record(self.profile, frame, record_motion)
|
||||||
# Pose estimation is deliberately outside the state lock. Start swaps
|
# Pose estimation is deliberately outside the state lock. Start swaps
|
||||||
# capture objects; pause/abort invalidates the epoch; movement changes
|
# capture objects; pause/abort invalidates the epoch; movement changes
|
||||||
# the motion identity. None of their late results may enter the session.
|
# the motion identity. None of their late results may enter the session.
|
||||||
@@ -497,6 +506,7 @@ class CalibrationCoordinator:
|
|||||||
return
|
return
|
||||||
sample_rows = [row for row in rows if row.get("kind") != "pnp_branch_event"
|
sample_rows = [row for row in rows if row.get("kind") != "pnp_branch_event"
|
||||||
and (record_motion is not None
|
and (record_motion is not None
|
||||||
|
or (self.raw_joint and row.get("kind") == "raw_observation_frame")
|
||||||
or (self.diagnostic_capture is not None and motion is not None
|
or (self.diagnostic_capture is not None and motion is not None
|
||||||
and motion.phase == "steady" and motion.minimum_hold_seconds > 0
|
and motion.phase == "steady" and motion.minimum_hold_seconds > 0
|
||||||
and row.get("kind") == "diagnostic_observation_frame")
|
and row.get("kind") == "diagnostic_observation_frame")
|
||||||
@@ -633,6 +643,26 @@ class CalibrationCoordinator:
|
|||||||
self.training_worker.close()
|
self.training_worker.close()
|
||||||
self._save_stage_timing()
|
self._save_stage_timing()
|
||||||
|
|
||||||
|
def _handle_motion_failure(self, decision, now):
|
||||||
|
if decision.code == 'mechanical_stall' and self.stall_retry is not None and self.segment is not None:
|
||||||
|
identity = (str(self._direction_progress.identity) if self._direction_progress is not None
|
||||||
|
else self.segment.identity)
|
||||||
|
retry = self.stall_retry.failed(identity, now)
|
||||||
|
limit = self.stall_retry.maximum_failures
|
||||||
|
append_jsonl(self.raw_path, dict(kind='motion_stall_retry',
|
||||||
|
stamp_ns=self.ports.clock_ns(), motion_id=identity,
|
||||||
|
failure_count=self.stall_retry.failures, maximum_failures=limit,
|
||||||
|
delay_seconds=self.stall_retry.delay_seconds if retry else 0.,
|
||||||
|
retry_scheduled=retry, target=list(self.segment.command.target), reason=decision.details))
|
||||||
|
if retry:
|
||||||
|
self._steady_capture_after_ns = None
|
||||||
|
self._steady_rows.clear()
|
||||||
|
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/{limit}:delay_seconds=3'
|
||||||
|
return
|
||||||
|
self._pause(f'{decision.code}:{decision.reason}:{decision.details},consecutive_failures={limit}')
|
||||||
|
return
|
||||||
|
self._pause(f"{decision.code}:{decision.reason}:{decision.details}")
|
||||||
|
|
||||||
def _advance(self):
|
def _advance(self):
|
||||||
if self.state in _TERMINAL_STATES:
|
if self.state in _TERMINAL_STATES:
|
||||||
return
|
return
|
||||||
@@ -667,33 +697,27 @@ class CalibrationCoordinator:
|
|||||||
motion_id=self.segment.identity if self.segment else "",
|
motion_id=self.segment.identity if self.segment else "",
|
||||||
motion_goals=self.segment.goals if self.segment else ()))
|
motion_goals=self.segment.goals if self.segment else ()))
|
||||||
if not decision.safe:
|
if not decision.safe:
|
||||||
if (decision.code == 'mechanical_stall' and self.stall_retry is not None
|
self._handle_motion_failure(decision, now)
|
||||||
and self.segment is not None):
|
|
||||||
retry = self.stall_retry.failed(self.segment.identity, now)
|
|
||||||
append_jsonl(self.raw_path, dict(kind='motion_stall_retry',
|
|
||||||
stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity,
|
|
||||||
failure_count=self.stall_retry.failures, maximum_failures=3,
|
|
||||||
delay_seconds=3.0 if retry else 0., retry_scheduled=retry,
|
|
||||||
target=list(self.segment.command.target), reason=decision.details))
|
|
||||||
if retry:
|
|
||||||
self._steady_capture_after_ns = None
|
|
||||||
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:delay_seconds=3'
|
|
||||||
return
|
|
||||||
self._pause(f'{decision.code}:{decision.reason}:{decision.details},consecutive_failures=3')
|
|
||||||
return
|
|
||||||
self._pause(f"{decision.code}:{decision.reason}:{decision.details}")
|
|
||||||
return
|
return
|
||||||
if self.stall_retry is not None and self.stall_retry.waiting:
|
if self.stall_retry is not None and self.stall_retry.waiting:
|
||||||
elapsed = self.stall_retry.resume(now)
|
elapsed = self.stall_retry.resume(now)
|
||||||
if elapsed is None:
|
if elapsed is None:
|
||||||
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:remaining={self.stall_retry.remaining(now):.1f}s'
|
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/{self.stall_retry.maximum_failures}:remaining={self.stall_retry.remaining(now):.1f}s'
|
||||||
return
|
return
|
||||||
self.segment.suspend_elapsed(elapsed)
|
self.segment.suspend_elapsed(elapsed)
|
||||||
|
if self._direction_progress is not None:
|
||||||
|
self._direction_progress.retry(now, self.latest_feedback)
|
||||||
self._steady_capture_after_ns = None
|
self._steady_capture_after_ns = None
|
||||||
self._steady_rows.clear()
|
self._steady_rows.clear()
|
||||||
append_jsonl(self.raw_path, dict(kind='motion_stall_retry_resumed',
|
append_jsonl(self.raw_path, dict(kind='motion_stall_retry_resumed',
|
||||||
stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity,
|
stamp_ns=self.ports.clock_ns(), motion_id=self.stall_retry.motion_id,
|
||||||
attempt=self.stall_retry.failures+1, waited_seconds=elapsed))
|
attempt=self.stall_retry.failures+1, waited_seconds=elapsed))
|
||||||
|
if self._direction_progress is not None:
|
||||||
|
progress = self._direction_progress.observe(self._motion, self.segment, current,
|
||||||
|
self.latest_feedback, self.state_history[-1].stamp_ns, now)
|
||||||
|
if not progress.safe:
|
||||||
|
self._handle_motion_failure(progress, now)
|
||||||
|
return
|
||||||
if self.visual_motion is not None and self.segment is not None:
|
if self.visual_motion is not None and self.segment is not None:
|
||||||
reason = self.visual_motion.check(self.segment, now)
|
reason = self.visual_motion.check(self.segment, now)
|
||||||
if reason:
|
if reason:
|
||||||
@@ -712,6 +736,7 @@ class CalibrationCoordinator:
|
|||||||
self.capture_index.extend(interrupted)
|
self.capture_index.extend(interrupted)
|
||||||
self.execution.completed_units.update(self._resume_checkpoint.completed_units)
|
self.execution.completed_units.update(self._resume_checkpoint.completed_units)
|
||||||
self._resumed_count = len(self._resume_checkpoint.completed_units)
|
self._resumed_count = len(self._resume_checkpoint.completed_units)
|
||||||
|
self._resume_message = '原始证据与补采预算已恢复，继续剩余方向'
|
||||||
self._raw_resume_import = None
|
self._raw_resume_import = None
|
||||||
return
|
return
|
||||||
if self._passed_reference_import is not None:
|
if self._passed_reference_import is not None:
|
||||||
@@ -755,6 +780,12 @@ class CalibrationCoordinator:
|
|||||||
self._verify_resume()
|
self._verify_resume()
|
||||||
return
|
return
|
||||||
if phase == Phase.EVALUATE:
|
if phase == Phase.EVALUATE:
|
||||||
|
if self._direction_progress is not None:
|
||||||
|
progress = self._direction_progress.finish()
|
||||||
|
self._direction_progress = None
|
||||||
|
if not progress.safe:
|
||||||
|
self._pause(f"{progress.code}:{progress.reason}:{progress.details}")
|
||||||
|
return
|
||||||
action = self.execution.action
|
action = self.execution.action
|
||||||
training_decision = None
|
training_decision = None
|
||||||
if self.execution.training_check_due():
|
if self.execution.training_check_due():
|
||||||
@@ -902,11 +933,21 @@ class CalibrationCoordinator:
|
|||||||
if self.visual_motion is not None:
|
if self.visual_motion is not None:
|
||||||
self.visual_motion.begin(motion, current, now=now, version=self._segment_number)
|
self.visual_motion.begin(motion, current, now=now, version=self._segment_number)
|
||||||
self._flush_visual_evidence()
|
self._flush_visual_evidence()
|
||||||
|
if self.raw_joint and motion.phase in {"sweep", "steady"} and motion.direction is not None:
|
||||||
|
unit = session.current_unit
|
||||||
|
identity = (*unit.identity, motion.attempt)
|
||||||
|
if self._direction_progress is None or self._direction_progress.identity != identity:
|
||||||
|
from .direction_progress import DirectionProgress
|
||||||
|
self._direction_progress = DirectionProgress(self.profile, unit, motion.attempt,
|
||||||
|
self.latest_feedback, now)
|
||||||
self.segment = MotionExecution(self.profile, motion, initial_command=current,
|
self.segment = MotionExecution(self.profile, motion, initial_command=current,
|
||||||
initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number),
|
initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number),
|
||||||
visual_observer=self.visual_motion)
|
visual_observer=self.visual_motion, raw_scan=self.raw_joint)
|
||||||
shaped = self.segment.sample(now)
|
shaped = self.segment.sample(now)
|
||||||
self._publish_command(list(shaped))
|
self._publish_command(list(shaped))
|
||||||
|
if self._direction_progress is not None:
|
||||||
|
self._direction_progress.observe(motion, self.segment, shaped,
|
||||||
|
self.latest_feedback, self.state_history[-1].stamp_ns, now)
|
||||||
self.segment.observe(self.latest_feedback, stamp=self.state_history[-1].stamp_ns if self.state_history else None, now=now)
|
self.segment.observe(self.latest_feedback, stamp=self.state_history[-1].stamp_ns if self.state_history else None, now=now)
|
||||||
if self.visual_motion is not None:
|
if self.visual_motion is not None:
|
||||||
self.visual_motion.settle_record(self.segment.finished_at, now, epoch=self._observation_epoch)
|
self.visual_motion.settle_record(self.segment.finished_at, now, epoch=self._observation_epoch)
|
||||||
@@ -920,25 +961,16 @@ class CalibrationCoordinator:
|
|||||||
self._steady_capture_after_ns = self.ports.clock_ns()
|
self._steady_capture_after_ns = self.ports.clock_ns()
|
||||||
from .scan_quality import observation_streams
|
from .scan_quality import observation_streams
|
||||||
task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key)
|
task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key)
|
||||||
steady_complete = all(len({r["image_stamp_ns"] for r in self._steady_rows
|
rows = self._steady_rows
|
||||||
if r.get(field) == joint and r.get("view") == spec.view
|
|
||||||
and r.get("sample_phase") == "steady"}) >= self.profile.acquisition.steady_minimum_samples
|
|
||||||
for field, joint, spec in observation_streams(self.profile, task)
|
|
||||||
if not motion.observed_joints or joint in motion.observed_joints)
|
|
||||||
if self.raw_joint:
|
if self.raw_joint:
|
||||||
from .raw_quality import coverage_records
|
from .raw_quality import coverage_records
|
||||||
rows = tuple(coverage_records(self.profile, session.current_unit,
|
rows = tuple(coverage_records(self.profile, session.current_unit,
|
||||||
motion.attempt, self._steady_rows))
|
motion.attempt, self._steady_rows))
|
||||||
steady_complete = all(len({r['image_stamp_ns'] for r in rows
|
steady_complete = all(len({r["image_stamp_ns"] for r in rows
|
||||||
if r.get(field) == joint and r['view'] == spec.view})
|
if r.get(field) == joint and r.get("view") == spec.view
|
||||||
>= self.profile.acquisition.steady_minimum_samples
|
and r.get("sample_phase") == "steady"}) >= self.profile.acquisition.steady_minimum_samples
|
||||||
for field, joint, spec in observation_streams(self.profile, task)
|
for field, joint, spec in observation_streams(self.profile, task)
|
||||||
if not motion.observed_joints or joint in motion.observed_joints)
|
if not motion.observed_joints or joint in motion.observed_joints)
|
||||||
# Missing images are a coverage failure after this bounded
|
|
||||||
# hold; they must not turn an acquisition visit into an
|
|
||||||
# unbounded wait for a geometry model.
|
|
||||||
if self.segment.arrived(now):
|
|
||||||
steady_complete = True
|
|
||||||
steady_complete = steady_complete and self.segment.minimum_hold_complete(now)
|
steady_complete = steady_complete and self.segment.minimum_hold_complete(now)
|
||||||
if self.chain_capture is not None:
|
if self.chain_capture is not None:
|
||||||
try:
|
try:
|
||||||
@@ -1047,6 +1079,9 @@ class CalibrationCoordinator:
|
|||||||
return
|
return
|
||||||
self._pause("joint_zero_unavailable:"+unavailable)
|
self._pause("joint_zero_unavailable:"+unavailable)
|
||||||
return
|
return
|
||||||
|
# All ordinary holds share this bounded exit, even without stable
|
||||||
|
# images. Unit evaluation owns missing-data retries; motion arrival
|
||||||
|
# still requires physical feedback progress.
|
||||||
if (self.segment.arrived(now) and motion.phase != "joint_zero") or steady_complete:
|
if (self.segment.arrived(now) and motion.phase != "joint_zero") or steady_complete:
|
||||||
if motion.phase in {'observer_preflight', 'observer_probe'}:
|
if motion.phase in {'observer_preflight', 'observer_probe'}:
|
||||||
self._advance_observer_preparation(motion, now)
|
self._advance_observer_preparation(motion, now)
|
||||||
@@ -1075,6 +1110,8 @@ class CalibrationCoordinator:
|
|||||||
if phase == Phase.REFERENCE_POSES and session.phase != phase:
|
if phase == Phase.REFERENCE_POSES and session.phase != phase:
|
||||||
self._save_reference()
|
self._save_reference()
|
||||||
self.segment, self._motion = None, None
|
self.segment, self._motion = None, None
|
||||||
|
if session.phase == Phase.CAPTURE_COMPLETE:
|
||||||
|
self._finish_raw_capture()
|
||||||
if session.phase == Phase.DIAGNOSTIC_COMPLETE:
|
if session.phase == Phase.DIAGNOSTIC_COMPLETE:
|
||||||
self._finish_diagnostic_capture()
|
self._finish_diagnostic_capture()
|
||||||
|
|
||||||
@@ -1092,6 +1129,27 @@ class CalibrationCoordinator:
|
|||||||
self._steady_capture_after_ns = None
|
self._steady_capture_after_ns = None
|
||||||
self._steady_rows.clear()
|
self._steady_rows.clear()
|
||||||
|
|
||||||
|
def _finish_raw_capture(self):
|
||||||
|
self._raw_capture_closed = True
|
||||||
|
self._observation_epoch += 1
|
||||||
|
self._direction_progress = None
|
||||||
|
self.journal_durability.close()
|
||||||
|
session = self.execution.session
|
||||||
|
try:
|
||||||
|
append_jsonl(self.raw_path, dict(kind="raw_capture_finished",
|
||||||
|
stamp_ns=self.ports.clock_ns(), returned_to_baseline=True))
|
||||||
|
sync_jsonl(self.raw_path)
|
||||||
|
from .raw_completion import save_capture_manifest
|
||||||
|
save_capture_manifest(self.raw_path, profile=self.profile,
|
||||||
|
serial_number=self.parameters.serial_number,
|
||||||
|
protected_inputs=self.parameters.protected_inputs,
|
||||||
|
completed_units=self.execution.completed_units,
|
||||||
|
incomplete_units=session.incomplete_units, retries=session._retry_by_unit)
|
||||||
|
self.reason = "raw_capture_finished_offline_only"
|
||||||
|
except Exception as error:
|
||||||
|
session.fail("raw_capture_manifest_failed", str(error))
|
||||||
|
self.reason = "raw_capture_manifest_failed:" + str(error)
|
||||||
|
|
||||||
def _finish_diagnostic_capture(self):
|
def _finish_diagnostic_capture(self):
|
||||||
"""Save diagnostics under the state lock before exposing completion."""
|
"""Save diagnostics under the state lock before exposing completion."""
|
||||||
from .diagnostic_report import build_diagnostic_report
|
from .diagnostic_report import build_diagnostic_report
|
||||||
@@ -1729,11 +1787,11 @@ class CalibrationCoordinator:
|
|||||||
return self.state_history[-1]
|
return self.state_history[-1]
|
||||||
|
|
||||||
def _publish_command(self, values):
|
def _publish_command(self, values):
|
||||||
if self.parameters.commands_enabled:
|
if self.parameters.commands_enabled and not self._raw_capture_closed:
|
||||||
self.sdk_adapter.publish_position(values)
|
self.sdk_adapter.publish_position(values)
|
||||||
|
|
||||||
def _write_position(self, values):
|
def _write_position(self, values):
|
||||||
if not self.parameters.commands_enabled:
|
if not self.parameters.commands_enabled or self._raw_capture_closed:
|
||||||
return
|
return
|
||||||
bounded = self.sdk_adapter.validate_command(values)
|
bounded = self.sdk_adapter.validate_command(values)
|
||||||
self.ports.publish_position(bounded)
|
self.ports.publish_position(bounded)
|
||||||
@@ -1755,7 +1813,11 @@ class CalibrationCoordinator:
|
|||||||
command_vector=list(bounded), direction_by_index=list(self._command_directions),
|
command_vector=list(bounded), direction_by_index=list(self._command_directions),
|
||||||
phase=motion.phase if motion else self.execution.session.phase.value,
|
phase=motion.phase if motion else self.execution.session.phase.value,
|
||||||
task_name=motion.task_key if motion else None,
|
task_name=motion.task_key if motion else None,
|
||||||
cycle=motion.cycle if motion else None),), durable=False)
|
cycle=motion.cycle if motion else None,
|
||||||
|
segment_key=motion.segment_key if motion else '',
|
||||||
|
direction=motion.direction if motion else None,
|
||||||
|
attempt=motion.attempt if motion else None,
|
||||||
|
command_index=motion.command_index if motion else None),), durable=False)
|
||||||
|
|
||||||
def _publish_speed(self, speed):
|
def _publish_speed(self, speed):
|
||||||
indices = self.profile.command.command_index_by_joint.values()
|
indices = self.profile.command.command_index_by_joint.values()
|
||||||
@@ -2010,13 +2072,17 @@ class CalibrationCoordinator:
|
|||||||
try:
|
try:
|
||||||
if self._resume_checkpoint is None:
|
if self._resume_checkpoint is None:
|
||||||
raise ValueError(self._resume_checkpoint_error or 'raw_checkpoint_missing')
|
raise ValueError(self._resume_checkpoint_error or 'raw_checkpoint_missing')
|
||||||
self._resume_checkpoint.verify_reference(self.profile, self._current_fingerprint)
|
verification = self._resume_checkpoint.verify_reference(self.profile, self._current_fingerprint,
|
||||||
|
require_installation=False)
|
||||||
from .joint_resume import ResumeJournalImport
|
from .joint_resume import ResumeJournalImport
|
||||||
self._raw_resume_import = ResumeJournalImport(self._resume_checkpoint.import_rows())
|
self._raw_resume_import = ResumeJournalImport(self._resume_checkpoint.import_rows())
|
||||||
self._resume_message = '新协议参考与安装检查通过，正在导入原始证据和补采预算'
|
self._resume_message = ('固定基准检查通过，活动标签差异已记录待离线复核；正在导入原始证据和补采预算'
|
||||||
|
if verification['requires_offline_review'] else
|
||||||
|
'固定基准与活动标签基准一致，正在导入原始证据和补采预算')
|
||||||
append_jsonl(self.raw_path, dict(kind='raw_resume_verified',
|
append_jsonl(self.raw_path, dict(kind='raw_resume_verified',
|
||||||
source_header=self._resume_checkpoint.header,
|
source_header=self._resume_checkpoint.header,
|
||||||
source_reference=self._resume_checkpoint.fixed_reference))
|
source_reference=self._resume_checkpoint.fixed_reference,
|
||||||
|
installation_verification=verification))
|
||||||
except (ValueError, KeyError, TypeError) as error:
|
except (ValueError, KeyError, TypeError) as error:
|
||||||
self._pause('raw_resume_rejected:' + str(error))
|
self._pause('raw_resume_rejected:' + str(error))
|
||||||
return
|
return
|
||||||
|
|||||||
@@ -0,0 +1,83 @@
|
|||||||
|
"""Feedback watchdog for one raw scan direction, across its bounded holds.
|
||||||
|
|
||||||
|
No command/encoder scale is inferred. Only real signed encoder progress
|
||||||
|
refreshes the motion-time budget; a waypoint transition never refreshes it.
|
||||||
|
"""
|
||||||
|
import math
|
||||||
|
|
||||||
|
from .motion_execution import feedback_tolerances
|
||||||
|
from .safety import SafetyDecision
|
||||||
|
from .trajectory import build_calibration_motion_command
|
||||||
|
|
||||||
|
|
||||||
|
class DirectionProgress:
|
||||||
|
def __init__(self, profile, unit, attempt, feedback, now):
|
||||||
|
self.profile = profile
|
||||||
|
self.identity = (*unit.identity, attempt)
|
||||||
|
task = next(t for t in profile.motion.tasks if t.key == unit.task_key)
|
||||||
|
self.start = build_calibration_motion_command(task, unit.start,
|
||||||
|
profile=profile, segment_key=unit.segment_key)
|
||||||
|
end = build_calibration_motion_command(task, unit.end,
|
||||||
|
profile=profile, segment_key=unit.segment_key)
|
||||||
|
self.thresholds = [max(2., .05*(hi-lo)) for lo, hi in
|
||||||
|
zip(profile.command.minimum_values, profile.command.maximum_values)]
|
||||||
|
self.directions = {i: math.copysign(1., b-a) for i, (a, b) in
|
||||||
|
enumerate(zip(self.start, end)) if abs(b-a) > self.thresholds[i]}
|
||||||
|
self.mark = {i: direction*feedback[i] for i, direction in self.directions.items()}
|
||||||
|
self.turning = dict(self.mark)
|
||||||
|
self.advancing = set()
|
||||||
|
self.travel = {i: 0. for i in self.directions}
|
||||||
|
self.elapsed = dict(self.travel)
|
||||||
|
self.active = set()
|
||||||
|
self.now = now
|
||||||
|
self.stamp = None
|
||||||
|
self.tolerances = feedback_tolerances(profile.command.unit)
|
||||||
|
|
||||||
|
def observe(self, motion, segment, command, feedback, stamp, now):
|
||||||
|
dt = max(0., now-self.now)
|
||||||
|
self.now = now
|
||||||
|
for i in self.active:
|
||||||
|
self.elapsed[i] += dt
|
||||||
|
if stamp != self.stamp:
|
||||||
|
self.stamp = stamp
|
||||||
|
for i, direction in self.directions.items():
|
||||||
|
value = direction*feedback[i]
|
||||||
|
self.turning[i] = min(self.turning[i], value)
|
||||||
|
self.travel[i] = max(self.travel[i], value-self.turning[i])
|
||||||
|
if value >= self.mark[i]+self.tolerances.resolution:
|
||||||
|
self.mark[i] = value
|
||||||
|
self.elapsed[i] = 0.
|
||||||
|
self.advancing.add(i)
|
||||||
|
elif i not in self.advancing and value < self.mark[i]:
|
||||||
|
# Follow the initial reversal only. Once advancing, a
|
||||||
|
# rebound inside the observed range is noise, not progress.
|
||||||
|
self.mark[i] = value
|
||||||
|
self.active = {i for i in self.directions
|
||||||
|
if motion is not None and motion.phase == 'sweep'
|
||||||
|
and segment is not None and segment.phase < 1.
|
||||||
|
and abs(command[i]-self.start[i]) > self.thresholds[i]
|
||||||
|
and abs(segment.deltas[i]) > 0.}
|
||||||
|
for i, elapsed in self.elapsed.items():
|
||||||
|
if elapsed >= self.profile.acquisition.stall_timeout_seconds:
|
||||||
|
return self._stop(i, feedback[i], elapsed)
|
||||||
|
return SafetyDecision(True)
|
||||||
|
|
||||||
|
def retry(self, now, feedback):
|
||||||
|
"""One explicitly budgeted retry; retain direction and actual travel."""
|
||||||
|
self.now = now
|
||||||
|
self.active.clear()
|
||||||
|
for i, direction in self.directions.items():
|
||||||
|
self.elapsed[i] = 0.
|
||||||
|
self.mark[i] = direction*feedback[i]
|
||||||
|
|
||||||
|
def finish(self):
|
||||||
|
for i, travel in self.travel.items():
|
||||||
|
if travel+self.tolerances.resolution < self.tolerances.minimum_travel:
|
||||||
|
return self._stop(i, None, self.elapsed[i])
|
||||||
|
return SafetyDecision(True)
|
||||||
|
|
||||||
|
def _stop(self, channel, feedback, elapsed):
|
||||||
|
return SafetyDecision(False, 'mechanical_stall', '扫描方向缺少真实同向反馈推进',
|
||||||
|
f'direction={self.identity},channel={channel},feedback={feedback},'
|
||||||
|
f'timeout_seconds={self.profile.acquisition.stall_timeout_seconds:g},'
|
||||||
|
f'active_no_progress_seconds={elapsed:.6f}')
|
||||||
@@ -30,7 +30,8 @@ ADAPTIVE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v12_adaptive_training"
|
|||||||
STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v13_staged_training"
|
STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v13_staged_training"
|
||||||
TASKWISE_STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v14_taskwise_staged_training"
|
TASKWISE_STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v14_taskwise_staged_training"
|
||||||
TASKWISE_TWO_ROUND_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v16_taskwise_bounded_recovery"
|
TASKWISE_TWO_ROUND_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v16_taskwise_bounded_recovery"
|
||||||
RAW_JOINT_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v17_raw_joint"
|
LEGACY_RAW_JOINT_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v17_raw_joint"
|
||||||
|
RAW_JOINT_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v18_raw_corners_immediate"
|
||||||
SUPPORTED_CAPTURE_SCHEDULE_VERSIONS = frozenset((CAPTURE_SCHEDULE_VERSION,
|
SUPPORTED_CAPTURE_SCHEDULE_VERSIONS = frozenset((CAPTURE_SCHEDULE_VERSION,
|
||||||
STAGED_CAPTURE_SCHEDULE_VERSION, TASKWISE_STAGED_CAPTURE_SCHEDULE_VERSION,
|
STAGED_CAPTURE_SCHEDULE_VERSION, TASKWISE_STAGED_CAPTURE_SCHEDULE_VERSION,
|
||||||
TASKWISE_TWO_ROUND_CAPTURE_SCHEDULE_VERSION, RAW_JOINT_CAPTURE_SCHEDULE_VERSION,
|
TASKWISE_TWO_ROUND_CAPTURE_SCHEDULE_VERSION, RAW_JOINT_CAPTURE_SCHEDULE_VERSION,
|
||||||
@@ -311,6 +312,7 @@ class CalibrationEngine:
|
|||||||
and session_start.get("acquisition_policy_version")
|
and session_start.get("acquisition_policy_version")
|
||||||
== ACQUISITION_POLICY_VERSION
|
== ACQUISITION_POLICY_VERSION
|
||||||
and session_start.get("capture_schedule_version") in (
|
and session_start.get("capture_schedule_version") in (
|
||||||
|
(capture_schedule_version(self.profile), LEGACY_RAW_JOINT_CAPTURE_SCHEDULE_VERSION) if self.profile.quality.training_policy == "raw_joint_2_plus_1" else
|
||||||
(capture_schedule_version(self.profile),) if (self.profile.quality.training_policy in {ADAPTIVE, *STAGED_POLICIES} or self.profile.acquisition.fixed_reference_mode == "stationary_image"
|
(capture_schedule_version(self.profile),) if (self.profile.quality.training_policy in {ADAPTIVE, *STAGED_POLICIES} 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 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(task.parent_reference is not None for task in self.profile.motion.tasks)
|
||||||
|
|||||||
@@ -71,8 +71,11 @@ class MotionCommand:
|
|||||||
|
|
||||||
class MotionExecution:
|
class MotionExecution:
|
||||||
def __init__(self, profile, command: MotionCommand, *, initial_command,
|
def __init__(self, profile, command: MotionCommand, *, initial_command,
|
||||||
initial_feedback, now: float, identity: str, visual_observer=None):
|
initial_feedback, now: float, identity: str, visual_observer=None, raw_scan=False):
|
||||||
self.profile, self.command, self.identity = profile, command, str(identity)
|
self.profile, self.command, self.identity = profile, command, str(identity)
|
||||||
|
from ..core.domain.capture_plan import raw_joint_capture
|
||||||
|
self.raw_scan = (raw_scan and raw_joint_capture(profile) and command.phase in {"sweep", "steady"}
|
||||||
|
and command.direction is not None and not command.feedback_targets)
|
||||||
self.visual = visual_observer
|
self.visual = visual_observer
|
||||||
if profile.vision_motion and visual_observer is None:
|
if profile.vision_motion and visual_observer is None:
|
||||||
raise ValueError("visual motion requires its independent image observer")
|
raise ValueError("visual motion requires its independent image observer")
|
||||||
@@ -204,7 +207,7 @@ class MotionExecution:
|
|||||||
|
|
||||||
@property
|
@property
|
||||||
def goals(self):
|
def goals(self):
|
||||||
goals = {} if self.profile.vision_motion else {
|
goals = {} if self.profile.vision_motion or self.raw_scan else {
|
||||||
i: MotionGoal(i, self._travel_origins[i], self._travel_origins[i]
|
i: MotionGoal(i, self._travel_origins[i], self._travel_origins[i]
|
||||||
+ math.copysign(self.required_feedback_travel[i], self.deltas[i]),
|
+ math.copysign(self.required_feedback_travel[i], self.deltas[i]),
|
||||||
self.resolution, self.resolution)
|
self.resolution, self.resolution)
|
||||||
@@ -231,6 +234,10 @@ class MotionExecution:
|
|||||||
|
|
||||||
def feedback_motion_complete(self):
|
def feedback_motion_complete(self):
|
||||||
"""Physical completion is shared by capture and transition decisions."""
|
"""Physical completion is shared by capture and transition decisions."""
|
||||||
|
if self.raw_scan:
|
||||||
|
# Raw waypoints are sample locations, not calibrated encoder goals.
|
||||||
|
# The coordinator checks physical progress over the whole direction.
|
||||||
|
return True
|
||||||
if self.visual is not None:
|
if self.visual is not None:
|
||||||
return self.visual.motion_complete()
|
return self.visual.motion_complete()
|
||||||
if not self.history:
|
if not self.history:
|
||||||
@@ -257,6 +264,11 @@ class MotionExecution:
|
|||||||
if (self.phase < 1 or self.finished_at is None or len(self.history) < 3
|
if (self.phase < 1 or self.finished_at is None or len(self.history) < 3
|
||||||
or not self.feedback_motion_complete()):
|
or not self.feedback_motion_complete()):
|
||||||
return False
|
return False
|
||||||
|
if self.raw_scan and self.command.phase == "sweep":
|
||||||
|
# Every raw waypoint is followed by a bounded steady effect.
|
||||||
|
# Waiting for stability here would duplicate that effect and
|
||||||
|
# create an unbounded wait before its timeout can even start.
|
||||||
|
return True
|
||||||
if self.command.phase in {"steady", "joint_zero"}:
|
if self.command.phase in {"steady", "joint_zero"}:
|
||||||
# Data sufficiency is evaluated after the direction; never wait
|
# Data sufficiency is evaluated after the direction; never wait
|
||||||
# indefinitely for a Tag. Missing physical motion is instead
|
# indefinitely for a Tag. Missing physical motion is instead
|
||||||
|
|||||||
@@ -17,10 +17,15 @@ def visible_roles(row: Mapping, tags: Mapping) -> set[str]:
|
|||||||
return visible
|
return visible
|
||||||
for role, item in observations.items():
|
for role, item in observations.items():
|
||||||
tag = tags.get(role)
|
tag = tags.get(role)
|
||||||
|
raw = row.get("observation_protocol") == "raw_joint_images_v2"
|
||||||
|
diagnostics = item.get("candidate_diagnostics", {}) if isinstance(item, Mapping) else {}
|
||||||
|
stamp = (item.get("image_stamp_ns") if raw else
|
||||||
|
diagnostics.get("observation_stamp_ns") if isinstance(diagnostics, Mapping) else None
|
||||||
|
) if isinstance(item, Mapping) else None
|
||||||
if (tag is None or not isinstance(item, Mapping)
|
if (tag is None or not isinstance(item, Mapping)
|
||||||
or not isinstance(item.get("candidate_diagnostics"), Mapping)
|
|
||||||
or item.get("tag_role") != role or item.get("tag_id") != tag.tag_id
|
or item.get("tag_role") != role or item.get("tag_id") != tag.tag_id
|
||||||
or item.get("candidate_diagnostics", {}).get("observation_stamp_ns") != row.get("image_stamp_ns")):
|
or stamp != row.get("image_stamp_ns")
|
||||||
|
or (raw and item.get("pose_source") != "raw_corners")):
|
||||||
continue
|
continue
|
||||||
try:
|
try:
|
||||||
points = np.asarray(item.get("corners_xy"), dtype=float)
|
points = np.asarray(item.get("corners_xy"), dtype=float)
|
||||||
|
|||||||
@@ -0,0 +1,119 @@
|
|||||||
|
"""Seal acquisition independently from solving or publishing a calibration."""
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from ..core.domain.capture_plan import CapturePlan, RAW_PROTOCOL
|
||||||
|
from ..storage import atomic_write_json
|
||||||
|
from .engine import CalibrationEngine
|
||||||
|
|
||||||
|
|
||||||
|
def file_digest(path):
|
||||||
|
with Path(path).open('rb') as stream:
|
||||||
|
return hashlib.file_digest(stream, 'sha256').hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def implementation_digest():
|
||||||
|
root = Path(__file__).resolve().parent.parent
|
||||||
|
digest = hashlib.sha256()
|
||||||
|
for path in sorted(root.rglob('*.py')):
|
||||||
|
digest.update(str(path.relative_to(root)).encode())
|
||||||
|
digest.update(path.read_bytes())
|
||||||
|
return digest.hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def snapshot_capture_inputs(config, directory):
|
||||||
|
"""Persist the effective product and protected files before any motion."""
|
||||||
|
import yaml
|
||||||
|
directory = Path(directory)
|
||||||
|
product = yaml.safe_load(config.path.read_text())
|
||||||
|
files = {}
|
||||||
|
for field, filename in (('source_urdf', 'capture_source.urdf'),
|
||||||
|
('camera_extrinsics', 'capture_extrinsics.yaml'),
|
||||||
|
('calibration_config', 'capture_calibration.yaml'), ('tag_config', 'capture_tags.yaml')):
|
||||||
|
source = Path(getattr(config, field))
|
||||||
|
contents = source.read_bytes()
|
||||||
|
if hashlib.sha256(contents).hexdigest() != getattr(config, field+'_sha256'):
|
||||||
|
raise ValueError('capture_input_changed_before_start:' + field)
|
||||||
|
target = directory/filename
|
||||||
|
target.write_bytes(contents)
|
||||||
|
product['artifacts'][field] = str(target.resolve())
|
||||||
|
files[filename] = file_digest(target)
|
||||||
|
from ..core.urdf.patch import materialize_relative_mesh_assets
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
for asset in materialize_relative_mesh_assets(source=config.source_urdf, output=directory.resolve(),
|
||||||
|
urdf_root=ET.parse(config.source_urdf).getroot()):
|
||||||
|
files[str(asset.relative_to(directory.resolve()))] = file_digest(asset)
|
||||||
|
profile = directory/'capture_profile.yaml'
|
||||||
|
if file_digest(profile) != config.profile_config_sha256:
|
||||||
|
raise ValueError('capture_profile_snapshot_changed')
|
||||||
|
product.update(profile_config=str(profile.resolve()), profile_config_sha256=config.profile_config_sha256,
|
||||||
|
output_root=str(config.output_root), resume_mode=config.resume_mode)
|
||||||
|
files[profile.name] = file_digest(profile)
|
||||||
|
for view, settings in config.cameras.items():
|
||||||
|
target = directory/f'capture_intrinsics_{view}.yaml'
|
||||||
|
target.write_bytes(Path(settings['camera_info']).read_bytes())
|
||||||
|
product['cameras'][view]['camera_info'] = str(target.resolve())
|
||||||
|
files[target.name] = file_digest(target)
|
||||||
|
target = directory/'capture_product.yaml'
|
||||||
|
target.write_text(yaml.safe_dump(product, sort_keys=False, allow_unicode=True))
|
||||||
|
files[target.name] = file_digest(target)
|
||||||
|
atomic_write_json(directory/'capture_inputs.json', dict(files=files, original_product=str(config.path)))
|
||||||
|
|
||||||
|
|
||||||
|
def _sealed_path(directory, name):
|
||||||
|
relative = Path(name)
|
||||||
|
path = (directory/relative).resolve()
|
||||||
|
if relative.is_absolute() or '..' in relative.parts or not path.is_relative_to(directory.resolve()):
|
||||||
|
raise ValueError('raw_capture_invalid_sealed_path:' + name)
|
||||||
|
return path
|
||||||
|
|
||||||
|
|
||||||
|
def save_capture_manifest(raw_path, *, profile, serial_number, protected_inputs,
|
||||||
|
completed_units, incomplete_units, retries):
|
||||||
|
directory = raw_path.parent
|
||||||
|
files = {raw_path, *directory.glob(raw_path.stem + '.*'),
|
||||||
|
*directory.glob('imported_evidence_*.zlib'), *directory.glob('capture_*.yaml'),
|
||||||
|
*directory.glob('capture_source.urdf'), *directory.glob('capture_inputs.json')}
|
||||||
|
snapshot = directory/'capture_inputs.json'
|
||||||
|
if snapshot.exists():
|
||||||
|
files.update(_sealed_path(directory, name) for name in json.loads(snapshot.read_text())['files'])
|
||||||
|
units = CalibrationEngine(profile).scan_units()
|
||||||
|
manifest = dict(protocol=RAW_PROTOCOL, capture_finished=True, returned_to_baseline=True,
|
||||||
|
raw_capture_complete=(len(completed_units) == len(units) and not incomplete_units),
|
||||||
|
accuracy_passed=False, publication_allowed=False, automatic_geometry_recapture=False,
|
||||||
|
serial_number=serial_number, capture_plan=CapturePlan.from_profile(profile).as_dict(),
|
||||||
|
protected_inputs=dict(protected_inputs), implementation_sha256=implementation_digest(),
|
||||||
|
units=[dict(identity=list(unit.identity), attempts=1+retries.get(unit.identity, 0),
|
||||||
|
passed=unit.identity in completed_units,
|
||||||
|
failures=list(incomplete_units.get(unit.identity, {}).get('failures', ()))) for unit in units],
|
||||||
|
files={str(p.resolve().relative_to(directory.resolve())): file_digest(p)
|
||||||
|
for p in sorted(files) if p.is_file()})
|
||||||
|
atomic_write_json(directory / 'raw_capture_manifest.json', manifest)
|
||||||
|
return manifest
|
||||||
|
|
||||||
|
|
||||||
|
def verify_capture_manifest(raw_path):
|
||||||
|
raw_path = Path(raw_path)
|
||||||
|
manifest = json.loads((raw_path.parent / 'raw_capture_manifest.json').read_text())
|
||||||
|
if (manifest.get('protocol') != RAW_PROTOCOL or manifest.get('capture_finished') is not True
|
||||||
|
or manifest.get('returned_to_baseline') is not True
|
||||||
|
or raw_path.name not in manifest.get('files', {})):
|
||||||
|
raise ValueError('raw_capture_manifest_invalid')
|
||||||
|
for name, digest in manifest['files'].items():
|
||||||
|
if file_digest(_sealed_path(raw_path.parent, name)) != digest:
|
||||||
|
raise ValueError('raw_capture_sealed_file_changed:' + name)
|
||||||
|
return manifest
|
||||||
|
|
||||||
|
|
||||||
|
def finish_capture(session):
|
||||||
|
"""Report sealed acquisition after shutdown; never import a solver."""
|
||||||
|
session = Path(session)
|
||||||
|
manifest = verify_capture_manifest(session/'raw_samples.jsonl')
|
||||||
|
result = dict(capture_finished=True, raw_capture_complete=manifest['raw_capture_complete'],
|
||||||
|
control_closed=True, fit_started=False, accuracy_passed=False, publication_allowed=False,
|
||||||
|
automatic_recapture=False, raw_samples=str(session/'raw_samples.jsonl'))
|
||||||
|
atomic_write_json(session/'capture_result.json', result)
|
||||||
|
print(f"采集结束，控制栈已关闭；数据完整：{result['raw_capture_complete']}。"
|
||||||
|
f"原始数据：{result['raw_samples']}；请使用 --offline-raw 单独拟合。", flush=True)
|
||||||
|
return 0 if result['raw_capture_complete'] else 3
|
||||||
@@ -2,7 +2,7 @@
|
|||||||
from collections import defaultdict
|
from collections import defaultdict
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
from ..core.domain.capture_plan import CapturePlan, raw_joint_capture, evidence_digest
|
from ..core.domain.capture_plan import CapturePlan, RAW_PROTOCOL, raw_joint_capture, evidence_digest
|
||||||
from ..core.domain.motion_path import record_scan_identity
|
from ..core.domain.motion_path import record_scan_identity
|
||||||
from ..core.geometry.frozen_evidence import freeze_evidence
|
from ..core.geometry.frozen_evidence import freeze_evidence
|
||||||
from .engine import CalibrationEngine
|
from .engine import CalibrationEngine
|
||||||
@@ -16,15 +16,21 @@ class RawCalibrationEvidence:
|
|||||||
completed_units: tuple
|
completed_units: tuple
|
||||||
sha256: str
|
sha256: str
|
||||||
missing_units: tuple = ()
|
missing_units: tuple = ()
|
||||||
|
capture_plan: dict | None = None
|
||||||
|
reassessed_units: tuple = ()
|
||||||
|
|
||||||
|
|
||||||
def partition_raw_capture(profile, records, *, require_complete=True):
|
def partition_raw_capture(profile, records, *, require_complete=True, reassess_quality=False):
|
||||||
"""Recompute image coverage; a persisted passed flag is never sufficient."""
|
"""Recompute image coverage; a persisted passed flag is never sufficient."""
|
||||||
if not raw_joint_capture(profile):
|
if not raw_joint_capture(profile):
|
||||||
raise ValueError('raw_capture_protocol_required')
|
raise ValueError('raw_capture_protocol_required')
|
||||||
headers = [r for r in records if r.get('kind') == 'session_start']
|
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(profile).as_dict():
|
plans = [CapturePlan.from_profile(profile).as_dict(legacy_raw=legacy) for legacy in (False, True)]
|
||||||
|
if len(headers) != 1 or headers[0].get('capture_plan') not in plans:
|
||||||
raise ValueError('raw_capture_plan_mismatch')
|
raise ValueError('raw_capture_plan_mismatch')
|
||||||
|
plan = headers[0]['capture_plan']
|
||||||
|
protocol = plan['observation_protocol']['version']
|
||||||
|
reassess_quality = reassess_quality and protocol == RAW_PROTOCOL
|
||||||
units = {u.identity: u for u in CalibrationEngine(profile).scan_units()}
|
units = {u.identity: u for u in CalibrationEngine(profile).scan_units()}
|
||||||
completions, groups, identities = {}, defaultdict(list), set()
|
completions, groups, identities = {}, defaultdict(list), set()
|
||||||
latest_attempts = {}
|
latest_attempts = {}
|
||||||
@@ -48,6 +54,8 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
|||||||
raise ValueError('raw_capture_completion_order_invalid')
|
raise ValueError('raw_capture_completion_order_invalid')
|
||||||
completions[unit] = row
|
completions[unit] = row
|
||||||
continue
|
continue
|
||||||
|
if row.get('observation_protocol', 'raw_joint_images_v1') != protocol:
|
||||||
|
raise ValueError('raw_capture_mixed_observation_protocol')
|
||||||
identity = row.get('view'), row.get('image_stamp_ns')
|
identity = row.get('view'), row.get('image_stamp_ns')
|
||||||
if (identity in identities or type(identity[1]) is not int or identity[1] <= 0
|
if (identity in identities or type(identity[1]) is not int or identity[1] <= 0
|
||||||
or identity[0] not in profile.vision.view_names):
|
or identity[0] not in profile.vision.view_names):
|
||||||
@@ -56,10 +64,11 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
|||||||
if row.get('is_accepted_joint_sample') is not False or row.get('input_is_rectified') is not True:
|
if row.get('is_accepted_joint_sample') is not False or row.get('input_is_rectified') is not True:
|
||||||
raise ValueError('raw_capture_image_contract_changed')
|
raise ValueError('raw_capture_image_contract_changed')
|
||||||
groups[unit, attempt].append(row)
|
groups[unit, attempt].append(row)
|
||||||
accepted, selected, failures, spans = [], [], {}, {}
|
accepted, selected, failures, spans, reassessed = [], [], {}, {}, []
|
||||||
for identity, unit in units.items():
|
for identity, unit in units.items():
|
||||||
completion = completions.get(identity)
|
completion = completions.get(identity)
|
||||||
if (completion is None or completion.get('passed') is not True
|
if (completion is None or (completion.get('passed') is not True and not reassess_quality)
|
||||||
|
or 'interrupted_attempt' in completion.get('failures', ())
|
||||||
or completion['attempt'] != latest_attempts[identity]):
|
or completion['attempt'] != latest_attempts[identity]):
|
||||||
failures[str(identity)] = ('scan_incomplete',)
|
failures[str(identity)] = ('scan_incomplete',)
|
||||||
continue
|
continue
|
||||||
@@ -68,6 +77,11 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
|||||||
if not quality.passed:
|
if not quality.passed:
|
||||||
failures[str(identity)] = quality.failures
|
failures[str(identity)] = quality.failures
|
||||||
continue
|
continue
|
||||||
|
if completion.get('passed') is not True:
|
||||||
|
reassessed.append(dict(unit=list(identity), attempt=completion['attempt'],
|
||||||
|
recorded_failures=list(completion.get('failures', ())),
|
||||||
|
policy='sent_command_endpoints_and_declared_segment_feedback_density',
|
||||||
|
recomputed_metrics=dict(quality.metrics)))
|
||||||
accepted.append(identity)
|
accepted.append(identity)
|
||||||
selected.extend(rows)
|
selected.extend(rows)
|
||||||
if failures and require_complete:
|
if failures and require_complete:
|
||||||
@@ -77,8 +91,8 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
|||||||
training = tuple(r for r in frozen if r['cycle'] in (0, 1))
|
training = tuple(r for r in frozen if r['cycle'] in (0, 1))
|
||||||
validation = tuple(r for r in frozen if r['cycle'] == profile.quality.holdout_cycle)
|
validation = tuple(r for r in frozen if r['cycle'] == profile.quality.holdout_cycle)
|
||||||
return RawCalibrationEvidence(training, validation, tuple(accepted), evidence_digest({
|
return RawCalibrationEvidence(training, validation, tuple(accepted), evidence_digest({
|
||||||
'plan': CapturePlan.from_profile(profile).sha256, 'rows': frozen}),
|
'plan': evidence_digest(plan), 'rows': frozen}),
|
||||||
tuple(dict(unit=key, failures=list(value)) for key, value in failures.items()))
|
tuple(dict(unit=key, failures=list(value)) for key, value in failures.items()), plan, tuple(reassessed))
|
||||||
|
|
||||||
|
|
||||||
def fitting_frames(profile, rows):
|
def fitting_frames(profile, rows):
|
||||||
|
|||||||
@@ -57,6 +57,11 @@ def coverage_records(profile, unit, attempt, records):
|
|||||||
|
|
||||||
|
|
||||||
def evaluate_raw_unit(profile, unit, attempt, records, *, first_cycle_spans):
|
def evaluate_raw_unit(profile, unit, attempt, records, *, first_cycle_spans):
|
||||||
|
from ..core.domain.capture_plan import RAW_PROTOCOL
|
||||||
|
records = tuple(records)
|
||||||
|
current_protocol = any(r.get('observation_protocol') == RAW_PROTOCOL for r in records)
|
||||||
return evaluate_capture_unit(profile, unit, attempt,
|
return evaluate_capture_unit(profile, unit, attempt,
|
||||||
tuple(coverage_records(profile, unit, attempt, records)),
|
tuple(coverage_records(profile, unit, attempt, records)),
|
||||||
first_cycle_spans=first_cycle_spans)
|
first_cycle_spans=first_cycle_spans,
|
||||||
|
include_steady_endpoints=current_protocol,
|
||||||
|
scale_segment_feedback_coverage=current_protocol)
|
||||||
|
|||||||
@@ -0,0 +1,143 @@
|
|||||||
|
"""Explicit, offline restart boundary after a moving tag is remounted.
|
||||||
|
|
||||||
|
The source journal remains untouched. Only an already completed prefix can be
|
||||||
|
reused, and every old observation of the remounted tag is excluded. This is a
|
||||||
|
draft workflow: unseen baseline installations cannot certify a passed release.
|
||||||
|
"""
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from ..core.domain.motion_path import record_scan_identity
|
||||||
|
from ..storage import append_jsonl_many, atomic_write_json, sync_jsonl
|
||||||
|
from .engine import CalibrationEngine
|
||||||
|
from .raw_completion import file_digest
|
||||||
|
from .raw_resume import RawCheckpoint
|
||||||
|
from .scan_quality import observation_streams
|
||||||
|
|
||||||
|
|
||||||
|
REMOUNT_RECORD = 'raw_tag_remount_checkpoint'
|
||||||
|
|
||||||
|
|
||||||
|
def remount_prefix(profile, checkpoint, tag_ids, model):
|
||||||
|
"""Keep the finite schedule prefix, including its original retry budgets."""
|
||||||
|
requested = frozenset(tag_ids)
|
||||||
|
tags = [tag for view in profile.vision.views for tag in view.tags
|
||||||
|
if tag.tag_id in requested]
|
||||||
|
if (not requested or {tag.tag_id for tag in tags} != requested
|
||||||
|
or any(tag.fixed_reference for tag in tags)):
|
||||||
|
raise ValueError('remount_requires_known_moving_tags')
|
||||||
|
from ..profiles.observations import compile_tag_links
|
||||||
|
links = compile_tag_links(profile, model)
|
||||||
|
def root(role):
|
||||||
|
link = links[role]
|
||||||
|
if link == model.root_link:
|
||||||
|
return None
|
||||||
|
return next((joint.name for joint in model._chain(model.parent_joint_by_child[link])
|
||||||
|
if joint.kind != 'fixed'), None)
|
||||||
|
roots = {root(tag.role) for tag in tags}
|
||||||
|
if None in roots:
|
||||||
|
raise ValueError('remount_requires_a_movable_finger_branch')
|
||||||
|
# The affected finger is recaptured as one installation. Incidental old
|
||||||
|
# views of its other tags must not bind the new chain to an old rest pose.
|
||||||
|
roles = {role for role in links if root(role) in roots}
|
||||||
|
declarations = [(i, row) for i, row in enumerate(checkpoint.rows) if row.get('kind') == REMOUNT_RECORD]
|
||||||
|
previous = None
|
||||||
|
if declarations:
|
||||||
|
index, previous = declarations[-1]
|
||||||
|
if (len(declarations) != 1 or set(previous['remounted_tag_ids']) != requested
|
||||||
|
or not set(previous['excluded_old_roles']) < roles
|
||||||
|
or any(row.get('kind') in {'raw_observation_frame', 'scan_unit_complete'}
|
||||||
|
for row in checkpoint.rows[index+1:])):
|
||||||
|
raise ValueError('remount_already_declared_use_normal_resume')
|
||||||
|
# A prepared checkpoint can narrow its retained auxiliary evidence
|
||||||
|
# before scanning starts. No attempted direction may be rewound.
|
||||||
|
tasks = {task.key for task in profile.motion.tasks
|
||||||
|
if any(roles & {spec.parent_role, spec.child_role}
|
||||||
|
for _, _, spec in observation_streams(profile, task))}
|
||||||
|
units = CalibrationEngine(profile).scan_units()
|
||||||
|
boundary = next((i for i, unit in enumerate(units) if unit.task_key in tasks), None)
|
||||||
|
if boundary is None or boundary == 0:
|
||||||
|
raise ValueError('remount_has_no_unaffected_prefix')
|
||||||
|
prefix = {unit.identity for unit in units[:boundary]}
|
||||||
|
if checkpoint.completed_units != prefix:
|
||||||
|
# Do not silently discard good work after the boundary, or rewind a
|
||||||
|
# failed unrelated task and thereby grant it more attempts.
|
||||||
|
raise ValueError('remount_requires_exact_completed_prefix')
|
||||||
|
completions = [i for i, row in enumerate(checkpoint.rows)
|
||||||
|
if row.get('kind') == 'scan_unit_complete' and record_scan_identity(row) in prefix]
|
||||||
|
end = max(completions) + 1
|
||||||
|
retained, excluded = [], previous['excluded_old_tag_observations'] if previous else 0
|
||||||
|
for row in checkpoint.rows[:end]:
|
||||||
|
if row.get('kind') == 'raw_observation_frame':
|
||||||
|
old_tags = row.get('tags', {})
|
||||||
|
removed = roles & old_tags.keys()
|
||||||
|
if removed:
|
||||||
|
excluded += len(removed)
|
||||||
|
row = {**row, 'tags': {key: value for key, value in old_tags.items() if key not in roles},
|
||||||
|
'excluded_remounted_tag_roles': sorted(removed | set(row.get('excluded_remounted_tag_roles', ())))}
|
||||||
|
elif row.get('kind') == 'session_start':
|
||||||
|
row = {**row, 'resume_checkpoint_requested': True, 'derived_remount_checkpoint': True}
|
||||||
|
retained.append(row)
|
||||||
|
report = dict(kind=REMOUNT_RECORD, version=1, remounted_tag_ids=sorted(requested),
|
||||||
|
excluded_old_roles=sorted(roles), affected_finger_roots=sorted(roots),
|
||||||
|
retained_units=[list(unit.identity) for unit in units[:boundary]],
|
||||||
|
restart_unit=list(units[boundary].identity), remaining_units=len(units)-boundary,
|
||||||
|
excluded_old_tag_observations=excluded, new_installation_attempt_limit=2,
|
||||||
|
replaced_attempts=[{key: row[key] for key in ('task_name', 'cycle', 'direction', 'attempt', 'passed', 'failures')
|
||||||
|
if key in row} for row in checkpoint.rows[end:] if row.get('kind') == 'scan_unit_complete'],
|
||||||
|
installation_verification='available_baseline_corners_and_fixed_reference',
|
||||||
|
publication_allowed=False)
|
||||||
|
if previous:
|
||||||
|
report['replaced_attempts'] = previous['replaced_attempts']
|
||||||
|
report['refined_unstarted_checkpoint'] = True
|
||||||
|
return tuple(retained), report
|
||||||
|
|
||||||
|
|
||||||
|
def prepare_remount_checkpoint(config, source, output, tag_ids):
|
||||||
|
"""Prepare a reviewable checkpoint without opening ROS or an SDK."""
|
||||||
|
from .artifacts.capture_validation import validate_capture_provenance
|
||||||
|
from .runner_support import protected_inputs
|
||||||
|
source, output = Path(source).resolve(), Path(output).resolve()
|
||||||
|
source = source/'raw_samples.jsonl' if source.is_dir() else source
|
||||||
|
if output == source.parent or output.exists():
|
||||||
|
raise ValueError('remount_output_must_be_new_directory')
|
||||||
|
receipt = source.parent/'remount_successor.json'
|
||||||
|
if receipt.exists():
|
||||||
|
raise ValueError('remount_already_prepared_use_normal_resume:' + str(receipt))
|
||||||
|
profile = config.calibration_contract.typed_profile
|
||||||
|
checkpoint = RawCheckpoint.load(profile, CalibrationEngine(profile), source, config.serial_number)
|
||||||
|
validate_capture_provenance(profile, config.serial_number, protected_inputs(config), checkpoint.rows,
|
||||||
|
allow_installation_draft=True)
|
||||||
|
from ..core.urdf.kinematics import UrdfKinematicModel
|
||||||
|
rows, report = remount_prefix(profile, checkpoint, tag_ids, UrdfKinematicModel(config.source_urdf))
|
||||||
|
files = sorted({source, *source.parent.glob(source.stem+'.*'),
|
||||||
|
*source.parent.glob('imported_evidence_*.zlib'), source.parent/'capture_profile.yaml'})
|
||||||
|
report['source_files'] = {str(path): file_digest(path) for path in files if path.is_file()}
|
||||||
|
report['source_session'] = str(source.parent)
|
||||||
|
output.mkdir(parents=True)
|
||||||
|
path = output/'raw_samples.jsonl'
|
||||||
|
append_jsonl_many(path, (*rows, report), durable=True)
|
||||||
|
sync_jsonl(path)
|
||||||
|
snapshot = source.parent/'capture_profile.yaml'
|
||||||
|
if snapshot.is_file():
|
||||||
|
(output/snapshot.name).write_bytes(snapshot.read_bytes())
|
||||||
|
# Recompute quality after filtering; the derived checkpoint must still
|
||||||
|
# restore exactly the same good prefix before it can be offered to motion.
|
||||||
|
verified = RawCheckpoint.load(profile, CalibrationEngine(profile), path, config.serial_number)
|
||||||
|
if verified.completed_units != checkpoint.completed_units:
|
||||||
|
raise ValueError('remount_changed_unaffected_quality')
|
||||||
|
for name, digest in report['source_files'].items():
|
||||||
|
if file_digest(name) != digest:
|
||||||
|
raise ValueError('remount_source_changed_during_preparation')
|
||||||
|
report['checkpoint_files'] = {p.name: file_digest(p) for p in output.iterdir() if p.is_file()}
|
||||||
|
atomic_write_json(output/'remount_checkpoint.json', report)
|
||||||
|
# Exclusive receipt prevents repeated invocation on this old installation
|
||||||
|
# from resetting the new installation's attempts. Ordinary restarts resume
|
||||||
|
# the newest journal and keep the existing attempt counters.
|
||||||
|
with receipt.open('x', encoding='utf-8') as stream:
|
||||||
|
json.dump(dict(checkpoint=str(output), tag_ids=sorted(set(tag_ids))), stream, indent=2)
|
||||||
|
stream.write('\n')
|
||||||
|
stream.flush()
|
||||||
|
os.fsync(stream.fileno())
|
||||||
|
return report
|
||||||
@@ -64,13 +64,22 @@ class RawCheckpoint:
|
|||||||
or headers[0].get('serial_number') != serial_number
|
or headers[0].get('serial_number') != serial_number
|
||||||
or not engine.resume_compatible(headers[0])):
|
or not engine.resume_compatible(headers[0])):
|
||||||
raise ValueError('raw_resume_identity_or_protocol_changed')
|
raise ValueError('raw_resume_identity_or_protocol_changed')
|
||||||
|
from ..core.domain.capture_plan import CapturePlan
|
||||||
|
if headers[0].get('capture_plan') != CapturePlan.from_profile(profile).as_dict():
|
||||||
|
raise ValueError('raw_resume_identity_or_protocol_changed')
|
||||||
evidence = partition_raw_capture(profile, rows, require_complete=False)
|
evidence = partition_raw_capture(profile, rows, require_complete=False)
|
||||||
checkpoint = cls(headers[0], references[0], rows, frozenset(evidence.completed_units))
|
checkpoint = cls(headers[0], references[0], rows, frozenset(evidence.completed_units))
|
||||||
# Validate order and retry budgets before any live motor command.
|
# Validate order and retry budgets before any live motor command.
|
||||||
checkpoint.restore(CalibrationSession(profile))
|
checkpoint.restore(CalibrationSession(profile))
|
||||||
return checkpoint
|
return checkpoint
|
||||||
|
|
||||||
def verify_reference(self, profile, current):
|
def verify_reference(self, profile, current, *, require_installation=True):
|
||||||
|
"""Check fixed references; audit moving baselines separately.
|
||||||
|
|
||||||
|
Equal SDK commands do not prove equal physical poses after a restart.
|
||||||
|
Raw acquisition records that ambiguity without authorizing a measured
|
||||||
|
zero or a shared tag mount. Offline release keeps the strict default.
|
||||||
|
"""
|
||||||
previous = {**self.fixed_reference,
|
previous = {**self.fixed_reference,
|
||||||
'acquisition_policy_version': current['acquisition_policy_version']}
|
'acquisition_policy_version': current['acquisition_policy_version']}
|
||||||
checker = ResumeVerifier(required_fixed_views=profile.vision.view_names,
|
checker = ResumeVerifier(required_fixed_views=profile.vision.view_names,
|
||||||
@@ -85,19 +94,40 @@ class RawCheckpoint:
|
|||||||
raise ValueError('raw_resume_reference_changed:' + str(decision.incompatible_fields))
|
raise ValueError('raw_resume_reference_changed:' + str(decision.incompatible_fields))
|
||||||
old = self.fixed_reference.get('raw_installation_reference', {})
|
old = self.fixed_reference.get('raw_installation_reference', {})
|
||||||
new = current.get('raw_installation_reference', {})
|
new = current.get('raw_installation_reference', {})
|
||||||
|
fixed_roles = {f'{view.name}:{tag.role}' for view in profile.vision.views
|
||||||
|
for tag in view.tags if tag.fixed_reference}
|
||||||
roles = {f"{r['view']}:{role}" for r in self.rows
|
roles = {f"{r['view']}:{role}" for r in self.rows
|
||||||
if r.get('kind') == 'raw_observation_frame'
|
if r.get('kind') == 'raw_observation_frame'
|
||||||
for role in r.get('tags', {})}
|
for role in r.get('tags', {})} - fixed_roles
|
||||||
|
from .raw_remount import REMOUNT_RECORD
|
||||||
|
draft_remount = any(r.get('kind') == REMOUNT_RECORD for r in self.rows)
|
||||||
|
checked, unavailable, changed, diagnostics = [], [], [], {}
|
||||||
for role in sorted(roles):
|
for role in sorted(roles):
|
||||||
if role not in old or role not in new:
|
if role not in old or role not in new:
|
||||||
|
if draft_remount or not require_installation:
|
||||||
|
unavailable.append(role)
|
||||||
|
continue
|
||||||
raise ValueError('raw_resume_installation_observation_missing:' + role)
|
raise ValueError('raw_resume_installation_observation_missing:' + role)
|
||||||
drift = np.linalg.norm(np.asarray(old[role]['corners_xy'])
|
drift = np.linalg.norm(np.asarray(old[role]['corners_xy'])
|
||||||
-np.asarray(new[role]['corners_xy']), axis=-1)
|
-np.asarray(new[role]['corners_xy']), axis=-1)
|
||||||
feedback = np.abs(np.asarray(old[role]['feedback'])-np.asarray(new[role]['feedback']))
|
feedback = np.abs(np.asarray(old[role]['feedback'])-np.asarray(new[role]['feedback']))
|
||||||
if (not np.all(np.isfinite(drift)) or np.max(drift)
|
if not np.all(np.isfinite(drift)) or not np.all(np.isfinite(feedback)):
|
||||||
> profile.acquisition.fixed_reference_maximum_drift_px
|
raise ValueError('raw_resume_installation_invalid:' + role)
|
||||||
or not np.all(np.isfinite(feedback)) or np.max(feedback) > 10):
|
diagnostics[role] = dict(maximum_corner_difference_px=float(np.max(drift)),
|
||||||
|
maximum_feedback_difference=float(np.max(feedback)))
|
||||||
|
if (np.max(drift) > profile.acquisition.fixed_reference_maximum_drift_px
|
||||||
|
or np.max(feedback) > 10):
|
||||||
|
if require_installation:
|
||||||
raise ValueError('raw_resume_installation_changed:' + role)
|
raise ValueError('raw_resume_installation_changed:' + role)
|
||||||
|
changed.append(role)
|
||||||
|
continue
|
||||||
|
checked.append(role)
|
||||||
|
verified = not (changed or unavailable)
|
||||||
|
return dict(checked_roles=checked, unavailable_roles=unavailable,
|
||||||
|
changed_roles=changed, baseline_diagnostics=diagnostics,
|
||||||
|
installation_verified=verified, requires_offline_review=not verified,
|
||||||
|
policy='moving_baseline_audit_v1',
|
||||||
|
draft_only=draft_remount or not verified, fixed_reference_verified=True)
|
||||||
|
|
||||||
def restore(self, session):
|
def restore(self, session):
|
||||||
"""Replay journal transitions, including failed and interrupted attempts."""
|
"""Replay journal transitions, including failed and interrupted attempts."""
|
||||||
@@ -138,4 +168,5 @@ class RawCheckpoint:
|
|||||||
|
|
||||||
def import_rows(self):
|
def import_rows(self):
|
||||||
return tuple(r for r in self.rows if r.get('kind') not in {
|
return tuple(r for r in self.rows if r.get('kind') not in {
|
||||||
'session_start', 'fixed_base_reference_locked', 'rectified_camera_model'})
|
'session_start', 'fixed_base_reference_locked', 'rectified_camera_model',
|
||||||
|
'raw_capture_finished'})
|
||||||
|
|||||||
@@ -11,6 +11,9 @@ def reason_zh(
|
|||||||
) -> tuple[str, str, str]:
|
) -> tuple[str, str, str]:
|
||||||
"""Map a common runtime reason to a stable code, explanation and action."""
|
"""Map a common runtime reason to a stable code, explanation and action."""
|
||||||
reason = str(status.get("reason", "unknown"))
|
reason = str(status.get("reason", "unknown"))
|
||||||
|
if 'o30_sdk_identity_mismatch' in reason or 'o30_sdk_device_changed' in reason:
|
||||||
|
return ('DEVICE-IDENTITY-203', 'SDK 报告的设备身份与当前 O30 配置不一致。',
|
||||||
|
'核对诊断中保存的型号、左右手、设备 UID 和通道名称，以及 SDK 信息话题是否存在其他发布者。')
|
||||||
if 'shared_pose_handoff_' in reason:
|
if 'shared_pose_handoff_' in reason:
|
||||||
return ('OBS-HANDOFF-120', f'共享 Tag 的任务交接证据不完整或已发生变化：{reason}',
|
return ('OBS-HANDOFF-120', f'共享 Tag 的任务交接证据不完整或已发生变化：{reason}',
|
||||||
'核对上游回基准的完整原始图像窗口、反馈及时间顺序；交接不能借用验证轮、跨会话旧图或挑选低误差帧。原零位和已通过数据保持冻结。')
|
'核对上游回基准的完整原始图像窗口、反馈及时间顺序；交接不能借用验证轮、跨会话旧图或挑选低误差帧。原零位和已通过数据保持冻结。')
|
||||||
@@ -170,6 +173,14 @@ def reason_zh(
|
|||||||
if reason.startswith("mechanical_stall:"):
|
if reason.startswith("mechanical_stall:"):
|
||||||
timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason)
|
timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason)
|
||||||
duration = f" {timeout.group(1)} 秒" if timeout else "两秒"
|
duration = f" {timeout.group(1)} 秒" if timeout else "两秒"
|
||||||
|
if 'consecutive_failures=2' in reason:
|
||||||
|
return ('MOTION-STALL-303',
|
||||||
|
f'停滞后已等待 3 秒并重试一次，仍未获得有效反馈推进（预算{duration}），已停止。',
|
||||||
|
'本次硬件重试已用尽；检查执行器、反馈和机械阻挡。')
|
||||||
|
if 'active_no_progress_seconds=' in reason:
|
||||||
|
return ('MOTION-STALL-303',
|
||||||
|
f'当前扫描方向缺少有效反馈推进，运动时间预算为{duration}（不计计划内静止采样），已保持当前位置。',
|
||||||
|
'检查机械阻挡、执行器和反馈；硬件重试预算由采集记录独立保存。')
|
||||||
if 'consecutive_failures=3' in reason:
|
if 'consecutive_failures=3' in reason:
|
||||||
return ('MOTION-STALL-303',
|
return ('MOTION-STALL-303',
|
||||||
f'同一运动连续 3 次失败，每次反馈{duration}未推进；两次重发前均等待 3 秒，已停止。',
|
f'同一运动连续 3 次失败，每次反馈{duration}未推进；两次重发前均等待 3 秒，已停止。',
|
||||||
|
|||||||
@@ -23,7 +23,7 @@ from ..storage import atomic_write_json
|
|||||||
from .status import normalize_status, render_status_zh
|
from .status import normalize_status, render_status_zh
|
||||||
|
|
||||||
|
|
||||||
TERMINAL_STATES = frozenset({"COMPLETE", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"})
|
TERMINAL_STATES = frozenset({"COMPLETE", "CAPTURE_COMPLETE", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"})
|
||||||
|
|
||||||
|
|
||||||
def drive_session(
|
def drive_session(
|
||||||
@@ -154,6 +154,15 @@ def run_online(
|
|||||||
from dataclasses import replace
|
from dataclasses import replace
|
||||||
config = replace(config, profile_config=effective_path,
|
config = replace(config, profile_config=effective_path,
|
||||||
profile_config_sha256=effective_digest)
|
profile_config_sha256=effective_digest)
|
||||||
|
if config.profile_config is not None:
|
||||||
|
captured_profile = Path(config.profile_config).read_bytes()
|
||||||
|
if hashlib.sha256(captured_profile).hexdigest() != config.profile_config_sha256:
|
||||||
|
raise ValueError('capture_profile_changed_before_start')
|
||||||
|
(session/'capture_profile.yaml').write_bytes(captured_profile)
|
||||||
|
from ..core.domain.capture_plan import raw_joint_capture
|
||||||
|
if raw_joint_capture(profile):
|
||||||
|
from .raw_completion import snapshot_capture_inputs
|
||||||
|
snapshot_capture_inputs(config, session)
|
||||||
from ..profiles.validator import validate_executable_profile, require_release_geometry
|
from ..profiles.validator import validate_executable_profile, require_release_geometry
|
||||||
contract_report = validate_executable_profile(profile, config.source_urdf)
|
contract_report = validate_executable_profile(profile, config.source_urdf)
|
||||||
atomic_write_json(session/"measurement_contract.json", contract_report)
|
atomic_write_json(session/"measurement_contract.json", contract_report)
|
||||||
@@ -171,6 +180,10 @@ def run_online(
|
|||||||
atomic_write_json(session/"resume_selection.json", dict(
|
atomic_write_json(session/"resume_selection.json", dict(
|
||||||
selected_session=resume.name if resume else None,
|
selected_session=resume.name if resume else None,
|
||||||
preparation_contract=preparation, rejected_candidates=rejected_checkpoints))
|
preparation_contract=preparation, rejected_candidates=rejected_checkpoints))
|
||||||
|
if resume is not None:
|
||||||
|
installation_note = resume/'operator_installation_clarification.json'
|
||||||
|
if installation_note.is_file():
|
||||||
|
(session/installation_note.name).write_bytes(installation_note.read_bytes())
|
||||||
from ..core.domain.capture_plan import raw_joint_capture
|
from ..core.domain.capture_plan import raw_joint_capture
|
||||||
if raw_joint_capture(profile) and allow_resume and rejected_checkpoints and resume is None:
|
if raw_joint_capture(profile) and allow_resume and rejected_checkpoints and resume is None:
|
||||||
raise ValueError("raw_resume_incompatible:no_automatic_recapture; 首次使用新协议请明确指定 --no-resume")
|
raise ValueError("raw_resume_incompatible:no_automatic_recapture; 首次使用新协议请明确指定 --no-resume")
|
||||||
@@ -259,6 +272,18 @@ def run_online(
|
|||||||
startup_timeout=startup_timeout,
|
startup_timeout=startup_timeout,
|
||||||
)
|
)
|
||||||
atomic_write_json(session / "node_status.json", status)
|
atomic_write_json(session / "node_status.json", status)
|
||||||
|
if normalize_status(status)["state"] == "CAPTURE_COMPLETE":
|
||||||
|
capture_complete = True
|
||||||
|
stop_stack(process)
|
||||||
|
process = None
|
||||||
|
monitor.destroy_node()
|
||||||
|
monitor = None
|
||||||
|
if owns_context and rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
log_stream.flush()
|
||||||
|
os.fsync(log_stream.fileno())
|
||||||
|
from .raw_completion import finish_capture
|
||||||
|
return finish_capture(session)
|
||||||
if diagnostic is not None:
|
if diagnostic is not None:
|
||||||
if normalize_status(status)["state"] != "DIAGNOSTIC_COMPLETE":
|
if normalize_status(status)["state"] != "DIAGNOSTIC_COMPLETE":
|
||||||
_write_failure(config, session, status)
|
_write_failure(config, session, status)
|
||||||
@@ -305,7 +330,7 @@ def run_online(
|
|||||||
deadline = time.monotonic() + 1.0
|
deadline = time.monotonic() + 1.0
|
||||||
while rclpy.ok() and not aborted.done() and time.monotonic() < deadline:
|
while rclpy.ok() and not aborted.done() and time.monotonic() < deadline:
|
||||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||||
message = "诊断采集已完成；已取消后续分析" if capture_complete else "已请求中止"
|
message = "采集已完成；已取消后续分析" if capture_complete else "已请求中止"
|
||||||
print(f"{message}；本次数据保留在 {session}。", flush=True)
|
print(f"{message}；本次数据保留在 {session}。", flush=True)
|
||||||
return 130
|
return 130
|
||||||
except Exception as error:
|
except Exception as error:
|
||||||
@@ -335,6 +360,10 @@ def main(args: list[str] | None = None) -> None:
|
|||||||
parser.add_argument("--record-bag", action="store_true")
|
parser.add_argument("--record-bag", action="store_true")
|
||||||
parser.add_argument("--commands-disabled", action="store_true")
|
parser.add_argument("--commands-disabled", action="store_true")
|
||||||
parser.add_argument("--no-resume", action="store_true")
|
parser.add_argument("--no-resume", action="store_true")
|
||||||
|
parser.add_argument("--prepare-remount-from", type=Path,
|
||||||
|
help="仅离线：保留未受影响的已完成前缀，为调整标签后的低精度草稿采集建立新断点")
|
||||||
|
parser.add_argument("--remounted-tag-id", type=int, action="append", default=[],
|
||||||
|
help="与 --prepare-remount-from 一起显式声明本次重新安装的移动标签，可重复指定")
|
||||||
parser.add_argument("--training-policy", choices=("fixed", "adaptive_2_to_3", "staged_2_to_3",
|
parser.add_argument("--training-policy", choices=("fixed", "adaptive_2_to_3", "staged_2_to_3",
|
||||||
"taskwise_staged_2_to_3", "taskwise_2_plus_1", "raw_joint_2_plus_1"), default=None,
|
"taskwise_staged_2_to_3", "taskwise_2_plus_1", "raw_joint_2_plus_1"), default=None,
|
||||||
help="taskwise_2_plus_1：每任务连续两轮训练加一轮验证、不补轮；其他策略保留原语义，默认沿用配置")
|
help="taskwise_2_plus_1：每任务连续两轮训练加一轮验证、不补轮；其他策略保留原语义，默认沿用配置")
|
||||||
@@ -355,11 +384,34 @@ def main(args: list[str] | None = None) -> None:
|
|||||||
help="当前安装下的多姿态同步标定板角点 JSON；新 O30 协议启动前重新计算光学核验")
|
help="当前安装下的多姿态同步标定板角点 JSON；新 O30 协议启动前重新计算光学核验")
|
||||||
parser.add_argument("--offline-output", default="")
|
parser.add_argument("--offline-output", default="")
|
||||||
parser.add_argument("--publish-offline", action="store_true")
|
parser.add_argument("--publish-offline", action="store_true")
|
||||||
|
parser.add_argument("--offline-draft", action="store_true",
|
||||||
|
help="仅离线：用已完成任务生成未认证的零位/行程修正 URDF，报告误差而不要求精度通过")
|
||||||
|
parser.add_argument("--capture-profile", type=Path,
|
||||||
|
help="草稿读取旧配置数据时提供原 Profile；仅允许离线修正标签安装连杆")
|
||||||
|
parser.add_argument("--draft-initialization-from", type=Path,
|
||||||
|
help="仅离线草稿：使用同一批数据的已有训练草稿作为初值，全部参数仍重新联合拟合")
|
||||||
|
parser.add_argument("--draft-upright-splay-zero", action="store_true",
|
||||||
|
help="仅离线草稿：按用户确认，将四指基准指令下的侧摆零位定义为 CAD 竖直；不作为独立实测零位")
|
||||||
parser.add_argument("--preparation-policy", nargs='?', const='occlusion_aware_trajectory_v3',
|
parser.add_argument("--preparation-policy", nargs='?', const='occlusion_aware_trajectory_v3',
|
||||||
choices=("occlusion_aware_witness_v1", "occlusion_aware_witness_v2",
|
choices=("occlusion_aware_witness_v1", "occlusion_aware_witness_v2",
|
||||||
"occlusion_aware_trajectory_v3"),
|
"occlusion_aware_trajectory_v3"),
|
||||||
help="O30 staged 准备策略；省略取值使用 v3 整段轨迹消歧和有界自动观察；v1/v2 仅保留历史兼容")
|
help="O30 staged 准备策略；省略取值使用 v3 整段轨迹消歧和有界自动观察；v1/v2 仅保留历史兼容")
|
||||||
selected = parser.parse_args(args)
|
selected = parser.parse_args(args)
|
||||||
|
if bool(selected.prepare_remount_from) != bool(selected.remounted_tag_id):
|
||||||
|
parser.error("--prepare-remount-from and --remounted-tag-id must be specified together")
|
||||||
|
if selected.prepare_remount_from and (selected.offline_raw or selected.offline_draft
|
||||||
|
or selected.publish_offline or selected.commands_disabled or selected.no_resume
|
||||||
|
or selected.validate_only or selected.diagnostic_capture or selected.diagnostic_thumb
|
||||||
|
or selected.diagnostic_yaw_hold):
|
||||||
|
parser.error("--prepare-remount-from is a separate offline operation")
|
||||||
|
if selected.offline_draft and (not selected.offline_raw or selected.publish_offline):
|
||||||
|
parser.error("--offline-draft requires --offline-raw and cannot publish a passed release")
|
||||||
|
if selected.capture_profile and not selected.offline_draft:
|
||||||
|
parser.error("--capture-profile requires --offline-draft")
|
||||||
|
if selected.draft_initialization_from and not selected.offline_draft:
|
||||||
|
parser.error("--draft-initialization-from requires --offline-draft")
|
||||||
|
if selected.draft_upright_splay_zero and not selected.offline_draft:
|
||||||
|
parser.error("--draft-upright-splay-zero requires --offline-draft")
|
||||||
if selected.startup_timeout_seconds is not None and (
|
if selected.startup_timeout_seconds is not None and (
|
||||||
not math.isfinite(selected.startup_timeout_seconds) or selected.startup_timeout_seconds <= 0
|
not math.isfinite(selected.startup_timeout_seconds) or selected.startup_timeout_seconds <= 0
|
||||||
):
|
):
|
||||||
@@ -372,7 +424,8 @@ def main(args: list[str] | None = None) -> None:
|
|||||||
parser.error(f"{diagnostic_flag} cannot be combined with --offline-raw, --offline-output, --publish-offline or --commands-disabled")
|
parser.error(f"{diagnostic_flag} cannot be combined with --offline-raw, --offline-output, --publish-offline or --commands-disabled")
|
||||||
config = load_product_config(
|
config = load_product_config(
|
||||||
selected.config or str(default_product_config_path()), workspace=selected.workspace,
|
selected.config or str(default_product_config_path()), workspace=selected.workspace,
|
||||||
check_can=not (selected.validate_only or selected.offline_raw or selected.commands_disabled),
|
check_can=not (selected.validate_only or selected.offline_raw or selected.commands_disabled
|
||||||
|
or selected.prepare_remount_from),
|
||||||
)
|
)
|
||||||
if selected.training_policy is not None:
|
if selected.training_policy is not None:
|
||||||
from dataclasses import replace
|
from dataclasses import replace
|
||||||
@@ -413,12 +466,32 @@ def main(args: list[str] | None = None) -> None:
|
|||||||
report = validate_executable_profile(config.calibration_contract.typed_profile, config.source_urdf)
|
report = validate_executable_profile(config.calibration_contract.typed_profile, config.source_urdf)
|
||||||
from ..core.domain.capture_plan import CapturePlan
|
from ..core.domain.capture_plan import CapturePlan
|
||||||
report["capture_plan"] = CapturePlan.from_profile(config.calibration_contract.typed_profile).as_dict()
|
report["capture_plan"] = CapturePlan.from_profile(config.calibration_contract.typed_profile).as_dict()
|
||||||
|
if config.profile_key.model == 'O30':
|
||||||
|
from ..core.fitting.raw_joint_draft import draft_targets
|
||||||
|
report['draft_correction_targets'] = draft_targets(config.calibration_contract.typed_profile)
|
||||||
if diagnostic is not None:
|
if diagnostic is not None:
|
||||||
report["diagnostic_capture"] = {**diagnostic.as_dict(), "publication_allowed": False}
|
report["diagnostic_capture"] = {**diagnostic.as_dict(), "publication_allowed": False}
|
||||||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
print(json.dumps(report, ensure_ascii=False, indent=2))
|
||||||
print(f"配置与受保护输入一致：{config.profile_key.profile_id}。此检查不代表 URDF 精度或硬件验收通过。")
|
print(f"配置与受保护输入一致：{config.profile_key.profile_id}。此检查不代表 URDF 精度或硬件验收通过。")
|
||||||
return
|
return
|
||||||
|
if selected.prepare_remount_from:
|
||||||
|
from datetime import datetime
|
||||||
|
from .raw_remount import prepare_remount_checkpoint
|
||||||
|
output = (Path(selected.offline_output) if selected.offline_output else
|
||||||
|
config.session_root/(datetime.now().strftime('%Y%m%d_%H%M%S')+'_remount'))
|
||||||
|
report = prepare_remount_checkpoint(config, selected.prepare_remount_from, output,
|
||||||
|
selected.remounted_tag_id)
|
||||||
|
print(f"标签调整后的草稿断点已保存：{output}；复用 {len(report['retained_units'])} 个方向，"
|
||||||
|
f"剩余 {report['remaining_units']} 个方向。原始记录未修改；后续使用普通断点恢复。")
|
||||||
|
return
|
||||||
if selected.offline_raw:
|
if selected.offline_raw:
|
||||||
|
if selected.offline_draft:
|
||||||
|
from .artifacts.raw_joint_draft import replay_draft
|
||||||
|
replay_draft(config, Path(selected.offline_raw),
|
||||||
|
output=Path(selected.offline_output) if selected.offline_output else None,
|
||||||
|
capture_profile=selected.capture_profile, initialization_from=selected.draft_initialization_from,
|
||||||
|
upright_splay_zero=selected.draft_upright_splay_zero)
|
||||||
|
return
|
||||||
from .artifacts.replay import replay_capture
|
from .artifacts.replay import replay_capture
|
||||||
replay_capture(config, Path(selected.offline_raw),
|
replay_capture(config, Path(selected.offline_raw),
|
||||||
output=Path(selected.offline_output) if selected.offline_output else None,
|
output=Path(selected.offline_output) if selected.offline_output else None,
|
||||||
|
|||||||
@@ -75,7 +75,7 @@ class SafetyPolicy:
|
|||||||
)
|
)
|
||||||
health = sample.health
|
health = sample.health
|
||||||
if not health.connected:
|
if not health.connected:
|
||||||
return self._stop("sdk_disconnected", "SDK 通信已失联")
|
return self._stop("sdk_disconnected", "SDK 连接或身份检查未通过", health.diagnostic)
|
||||||
if not health.position_mode:
|
if not health.position_mode:
|
||||||
return self._stop("wrong_control_mode", "机械手不在位置控制模式")
|
return self._stop("wrong_control_mode", "机械手不在位置控制模式")
|
||||||
if health.active_faults:
|
if health.active_faults:
|
||||||
|
|||||||
@@ -17,6 +17,8 @@ from ..core.domain.profile import observation_streams as observation_streams
|
|||||||
def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
||||||
records: Sequence[Mapping], *, first_cycle_spans: dict[str, float],
|
records: Sequence[Mapping], *, first_cycle_spans: dict[str, float],
|
||||||
include_steady: bool = True,
|
include_steady: bool = True,
|
||||||
|
include_steady_endpoints: bool = False,
|
||||||
|
scale_segment_feedback_coverage: bool = False,
|
||||||
) -> SweepQuality:
|
) -> SweepQuality:
|
||||||
"""No rates, coupling, endpoint tracking error or Tag percentages stop a scan.
|
"""No rates, coupling, endpoint tracking error or Tag percentages stop a scan.
|
||||||
|
|
||||||
@@ -66,7 +68,8 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
|||||||
samples, command_samples, feedback_samples = {}, {}, {}
|
samples, command_samples, feedback_samples = {}, {}, {}
|
||||||
channel = channel_for_joint(profile, name)
|
channel = channel_for_joint(profile, name)
|
||||||
for row in records:
|
for row in records:
|
||||||
if row.get("sample_phase", "sweep") != "sweep":
|
sample_phase = row.get("sample_phase", "sweep")
|
||||||
|
if sample_phase != "sweep" and not (include_steady_endpoints and sample_phase == "steady"):
|
||||||
continue
|
continue
|
||||||
if (row.get(field) != name or row.get("view") != spec.view
|
if (row.get(field) != name or row.get("view") != spec.view
|
||||||
or row.get("task_name") != unit.task_key
|
or row.get("task_name") != unit.task_key
|
||||||
@@ -82,6 +85,7 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
|||||||
profile.command.minimum_feedback_values[channel] <= float(feedback)
|
profile.command.minimum_feedback_values[channel] <= float(feedback)
|
||||||
<= profile.command.maximum_feedback_values[channel]):
|
<= profile.command.maximum_feedback_values[channel]):
|
||||||
continue
|
continue
|
||||||
|
if sample_phase == "sweep":
|
||||||
feedback_samples[(spec.view, stamp)] = float(feedback)
|
feedback_samples[(spec.view, stamp)] = float(feedback)
|
||||||
value = row.get(f"{'command' if command_coverage else 'feedback'}_{native}")
|
value = row.get(f"{'command' if command_coverage else 'feedback'}_{native}")
|
||||||
if value is None or not math.isfinite(float(value)):
|
if value is None or not math.isfinite(float(value)):
|
||||||
@@ -95,6 +99,10 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
|||||||
command_progress = (float(commanded) - lower) / span
|
command_progress = (float(commanded) - lower) / span
|
||||||
if 0 <= command_progress <= 1:
|
if 0 <= command_progress <= 1:
|
||||||
command_samples[(spec.view, stamp)] = command_progress
|
command_samples[(spec.view, stamp)] = command_progress
|
||||||
|
if sample_phase == "steady":
|
||||||
|
# A synchronized held image proves the actual sent endpoint
|
||||||
|
# command. It does not become a moving coverage sample.
|
||||||
|
continue
|
||||||
progress = (float(value) - lower) / span
|
progress = (float(value) - lower) / span
|
||||||
# Physical encoder bounds are distinct from the requested sweep.
|
# Physical encoder bounds are distinct from the requested sweep.
|
||||||
# Samples outside it are not clipped into fake endpoint bins.
|
# Samples outside it are not clipped into fake endpoint bins.
|
||||||
@@ -117,7 +125,10 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
|||||||
if command_coverage:
|
if command_coverage:
|
||||||
metrics[identity] = _command_coverage(metrics[identity], native)
|
metrics[identity] = _command_coverage(metrics[identity], native)
|
||||||
if not profile.vision_motion:
|
if not profile.vision_motion:
|
||||||
motion_quality, motion_metrics = _feedback_motion_coverage(profile, feedback_samples.values())
|
fraction = (span/(profile.command.maximum_values[channel]-profile.command.minimum_values[channel])
|
||||||
|
if segment is not None and scale_segment_feedback_coverage else 1.)
|
||||||
|
motion_quality, motion_metrics = _feedback_motion_coverage(profile, feedback_samples.values(),
|
||||||
|
command_span_fraction=fraction)
|
||||||
failures.extend(f"{identity}:feedback_motion_{reason}" for reason in motion_quality.failures)
|
failures.extend(f"{identity}:feedback_motion_{reason}" for reason in motion_quality.failures)
|
||||||
metrics[identity]["feedback_motion"] = motion_metrics
|
metrics[identity]["feedback_motion"] = motion_metrics
|
||||||
references[identity] = float(decision.metrics["feedback_span"])
|
references[identity] = float(decision.metrics["feedback_span"])
|
||||||
@@ -129,7 +140,7 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
|||||||
return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics)
|
return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics)
|
||||||
|
|
||||||
|
|
||||||
def _feedback_motion_coverage(profile, values):
|
def _feedback_motion_coverage(profile, values, *, command_span_fraction=1.):
|
||||||
"""Check actual encoder travel without interpreting commands as encoders.
|
"""Check actual encoder travel without interpreting commands as encoders.
|
||||||
|
|
||||||
Runtime checks direction, progress, and stability. The recorded direction
|
Runtime checks direction, progress, and stability. The recorded direction
|
||||||
@@ -142,15 +153,24 @@ def _feedback_motion_coverage(profile, values):
|
|||||||
span = upper-lower if values else 0.
|
span = upper-lower if values else 0.
|
||||||
normalized = tuple((value-lower)/span if span > 0 else 0. for value in values)
|
normalized = tuple((value-lower)/span if span > 0 else 0. for value in values)
|
||||||
policy = profile.acquisition
|
policy = profile.acquisition
|
||||||
|
if not 0 < command_span_fraction <= 1:
|
||||||
|
raise ValueError('feedback_coverage_invalid_declared_segment_span')
|
||||||
|
# A short declared segment cannot resolve as many native encoder bins as
|
||||||
|
# a full sweep. Scaling its sample-density check avoids amplifying byte
|
||||||
|
# quantization when the observed travel is normalized back to 256 bins.
|
||||||
|
minimum_bins = max(2, math.ceil(policy.minimum_bins*command_span_fraction))
|
||||||
|
maximum_gap_fraction = min(1., policy.maximum_unobserved_fraction/command_span_fraction)
|
||||||
quality = evaluate_observability(normalized, minimum_span=1.,
|
quality = evaluate_observability(normalized, minimum_span=1.,
|
||||||
minimum_valid_samples=policy.minimum_valid_samples, minimum_bins=policy.minimum_bins,
|
minimum_valid_samples=policy.minimum_valid_samples, minimum_bins=minimum_bins,
|
||||||
maximum_unobserved_fraction=policy.maximum_unobserved_fraction,
|
maximum_unobserved_fraction=maximum_gap_fraction,
|
||||||
total_frames=len(values), joint_frame_rate=1., feedback_hz=0., detection_rate=1.)
|
total_frames=len(values), joint_frame_rate=1., feedback_hz=0., detection_rate=1.)
|
||||||
if span < feedback_tolerances(profile.command.unit).minimum_travel:
|
if span < feedback_tolerances(profile.command.unit).minimum_travel:
|
||||||
quality = SweepQuality(False, (*quality.failures, "below_motion_resolution"),
|
quality = SweepQuality(False, (*quality.failures, "below_motion_resolution"),
|
||||||
quality.warnings, quality.metrics)
|
quality.warnings, quality.metrics)
|
||||||
return quality, dict(observed_minimum=lower, observed_maximum=upper, observed_span=span,
|
return quality, dict(observed_minimum=lower, observed_maximum=upper, observed_span=span,
|
||||||
unit=profile.command.unit, normalized_to="observed_feedback_interval",
|
unit=profile.command.unit, normalized_to="observed_feedback_interval",
|
||||||
|
declared_command_span_fraction=command_span_fraction, required_bins=minimum_bins,
|
||||||
|
allowed_maximum_bin_gap=quality.metrics['allowed_maximum_bin_gap'],
|
||||||
valid_frames=quality.metrics["valid_frames"], bins=quality.metrics["feedback_bins"],
|
valid_frames=quality.metrics["valid_frames"], bins=quality.metrics["feedback_bins"],
|
||||||
maximum_bin_gap=quality.metrics["maximum_bin_gap"])
|
maximum_bin_gap=quality.metrics["maximum_bin_gap"])
|
||||||
|
|
||||||
|
|||||||
@@ -38,6 +38,7 @@ class CalibrationPhase(str, Enum):
|
|||||||
PUBLISH = "PUBLISH"
|
PUBLISH = "PUBLISH"
|
||||||
COMPLETE = "COMPLETE"
|
COMPLETE = "COMPLETE"
|
||||||
DIAGNOSTIC_COMPLETE = "DIAGNOSTIC_COMPLETE"
|
DIAGNOSTIC_COMPLETE = "DIAGNOSTIC_COMPLETE"
|
||||||
|
CAPTURE_COMPLETE = "CAPTURE_COMPLETE"
|
||||||
PAUSED = "PAUSED"
|
PAUSED = "PAUSED"
|
||||||
ABORTED = "ABORTED"
|
ABORTED = "ABORTED"
|
||||||
FAILED = "FAILED"
|
FAILED = "FAILED"
|
||||||
@@ -217,7 +218,6 @@ class CalibrationSession:
|
|||||||
if raw_joint_capture(self.profile) and not quality.passed:
|
if raw_joint_capture(self.profile) and not quality.passed:
|
||||||
if self.engine.permits_retry("sweep_acquisition", retries):
|
if self.engine.permits_retry("sweep_acquisition", retries):
|
||||||
self._retry_by_unit[key] = retries + 1
|
self._retry_by_unit[key] = retries + 1
|
||||||
self._units.append(self._units.pop(self._unit_index))
|
|
||||||
self.phase = CalibrationPhase.PREPARE
|
self.phase = CalibrationPhase.PREPARE
|
||||||
else:
|
else:
|
||||||
self.incomplete_units[key] = {"failures": quality.failures,
|
self.incomplete_units[key] = {"failures": quality.failures,
|
||||||
@@ -350,6 +350,10 @@ class CalibrationSession:
|
|||||||
|
|
||||||
def return_complete(self) -> None:
|
def return_complete(self) -> None:
|
||||||
self._require(CalibrationPhase.RETURN_BASELINE)
|
self._require(CalibrationPhase.RETURN_BASELINE)
|
||||||
|
from ..core.domain.capture_plan import raw_joint_capture
|
||||||
|
if raw_joint_capture(self.profile) and self.diagnostic_capture is None:
|
||||||
|
self.phase = CalibrationPhase.CAPTURE_COMPLETE
|
||||||
|
return
|
||||||
if self.incomplete_units:
|
if self.incomplete_units:
|
||||||
self.fail("raw_capture_incomplete", "原始采集已归位，部分扫描补采后仍不完整",
|
self.fail("raw_capture_incomplete", "原始采集已归位，部分扫描补采后仍不完整",
|
||||||
{str(key): value for key, value in self.incomplete_units.items()},
|
{str(key): value for key, value in self.incomplete_units.items()},
|
||||||
@@ -433,9 +437,10 @@ class CalibrationSession:
|
|||||||
),
|
),
|
||||||
None,
|
None,
|
||||||
)
|
)
|
||||||
|
segment = task_segment(task, unit.segment_key) if task is not None and unit.segment_key else None
|
||||||
progress = (
|
progress = (
|
||||||
1.0
|
1.0
|
||||||
if self.phase in {CalibrationPhase.COMPLETE, CalibrationPhase.DIAGNOSTIC_COMPLETE}
|
if self.phase in {CalibrationPhase.COMPLETE, CalibrationPhase.DIAGNOSTIC_COMPLETE, CalibrationPhase.CAPTURE_COMPLETE}
|
||||||
else 0.90*self._unit_index / max(1, len(self._units))
|
else 0.90*self._unit_index / max(1, len(self._units))
|
||||||
)
|
)
|
||||||
required: dict[str, tuple[int, ...]] = {}
|
required: dict[str, tuple[int, ...]] = {}
|
||||||
@@ -443,7 +448,7 @@ class CalibrationSession:
|
|||||||
required = {view.name: tuple(tag.tag_id for tag in view.tags if tag.fixed_reference)
|
required = {view.name: tuple(tag.tag_id for tag in view.tags if tag.fixed_reference)
|
||||||
for view in self.profile.vision.views if view.name in self.required_observation_views}
|
for view in self.profile.vision.views if view.name in self.required_observation_views}
|
||||||
if task is not None:
|
if task is not None:
|
||||||
measurement_names = set(task_segment(task, unit.segment_key).joints if unit.segment_key else task.joints)
|
measurement_names = set(segment.joints if segment else task.joints)
|
||||||
measurement_names.update(
|
measurement_names.update(
|
||||||
self.profile.measurement.cross_view_sources[name]
|
self.profile.measurement.cross_view_sources[name]
|
||||||
for name in tuple(measurement_names)
|
for name in tuple(measurement_names)
|
||||||
@@ -489,12 +494,16 @@ class CalibrationSession:
|
|||||||
),
|
),
|
||||||
task=TaskStatus(
|
task=TaskStatus(
|
||||||
key="" if task is None else task.key,
|
key="" if task is None else task.key,
|
||||||
label="" if task is None else " / ".join(task.joints),
|
label="" if task is None else " / ".join(segment.joints if segment else task.joints),
|
||||||
cycle=displayed_cycle,
|
cycle=displayed_cycle,
|
||||||
cycle_count=displayed_cycle_count,
|
cycle_count=displayed_cycle_count,
|
||||||
direction="" if unit is None else unit.direction,
|
direction="" if unit is None else unit.direction,
|
||||||
capture_stage=capture_stage,
|
capture_stage=capture_stage,
|
||||||
required_tag_ids_by_view=required,
|
required_tag_ids_by_view=required,
|
||||||
|
segment_key="" if segment is None else segment.key,
|
||||||
|
segment_commands={} if segment is None else {
|
||||||
|
self.profile.command.names[index]: (start, dict(segment.end_commands)[index])
|
||||||
|
for index, start in sorted(segment.start_commands)},
|
||||||
),
|
),
|
||||||
resume={"message": self._resume_message,
|
resume={"message": self._resume_message,
|
||||||
**({"pending_unit_recaptures": [list(candidate.identity)
|
**({"pending_unit_recaptures": [list(candidate.identity)
|
||||||
|
|||||||
@@ -18,6 +18,7 @@ if TYPE_CHECKING:
|
|||||||
_LEGACY_STATES = {
|
_LEGACY_STATES = {
|
||||||
Phase.WAIT_DEVICE: "WAIT_DEVICES", Phase.READY: "READY",
|
Phase.WAIT_DEVICE: "WAIT_DEVICES", Phase.READY: "READY",
|
||||||
Phase.COMPLETE: "PASSED", Phase.PAUSED: "PAUSED",
|
Phase.COMPLETE: "PASSED", Phase.PAUSED: "PAUSED",
|
||||||
|
Phase.CAPTURE_COMPLETE: "CAPTURE_COMPLETE",
|
||||||
Phase.DIAGNOSTIC_COMPLETE: "DIAGNOSTIC_COMPLETE",
|
Phase.DIAGNOSTIC_COMPLETE: "DIAGNOSTIC_COMPLETE",
|
||||||
Phase.ABORTED: "ABORTED", Phase.FAILED: "FAILED",
|
Phase.ABORTED: "ABORTED", Phase.FAILED: "FAILED",
|
||||||
**{phase: "FINALIZING" for phase in (
|
**{phase: "FINALIZING" for phase in (
|
||||||
@@ -193,7 +194,7 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot:
|
|||||||
parameters.serial_number, host.reason, str(parameters.session_dir),
|
parameters.serial_number, host.reason, str(parameters.session_dir),
|
||||||
dict(host.reference_lock.latest_drift_px),
|
dict(host.reference_lock.latest_drift_px),
|
||||||
devices, steady, steady_node, preflight,
|
devices, steady, steady_node, preflight,
|
||||||
diagnostic, (dict(protocol='raw_joint_images_v1', formal_rounds_per_task=3,
|
diagnostic, (dict(protocol='raw_joint_images_v2', capture_finished=session.phase == Phase.CAPTURE_COMPLETE, formal_rounds_per_task=3,
|
||||||
actual_commands_sent=host._actual_command_count,
|
actual_commands_sent=host._actual_command_count,
|
||||||
actual_direction_changes_by_channel=list(host._actual_direction_changes),
|
actual_direction_changes_by_channel=list(host._actual_direction_changes),
|
||||||
command_count_scope='all_sent_commands_including_approach_avoidance_return_and_retries',
|
command_count_scope='all_sent_commands_including_approach_avoidance_return_and_retries',
|
||||||
|
|||||||
@@ -11,6 +11,10 @@ from .reporting.reasons_zh import reason_zh
|
|||||||
|
|
||||||
|
|
||||||
_STATES = {"WAIT_START": "READY", "WAIT_DEVICES": "WAIT_DEVICE", "PASSED": "COMPLETE"}
|
_STATES = {"WAIT_START": "READY", "WAIT_DEVICES": "WAIT_DEVICE", "PASSED": "COMPLETE"}
|
||||||
|
_SEGMENTS = {
|
||||||
|
"linked_increasing": "四指正向联动", "linked_decreasing": "四指反向联动",
|
||||||
|
"middle_lower_increasing": "中指向上补段", "middle_lower_decreasing": "中指向下补段",
|
||||||
|
}
|
||||||
_PHASES = {
|
_PHASES = {
|
||||||
"PREPARING_CHECKPOINT": "加载并校验断点证据（未开始运动）",
|
"PREPARING_CHECKPOINT": "加载并校验断点证据（未开始运动）",
|
||||||
"WAIT_DEVICE": "等待设备", "READY": "设备就绪", "BASELINE": "安全恢复基准",
|
"WAIT_DEVICE": "等待设备", "READY": "设备就绪", "BASELINE": "安全恢复基准",
|
||||||
@@ -22,6 +26,7 @@ _PHASES = {
|
|||||||
"FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "独立验证",
|
"FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "独立验证",
|
||||||
"BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF",
|
"BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF",
|
||||||
"PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停",
|
"PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停",
|
||||||
|
"CAPTURE_COMPLETE": "采集已结束（URDF 尚未验收）",
|
||||||
"DIAGNOSTIC_COMPLETE": "诊断采集完成（未进行精度验收）",
|
"DIAGNOSTIC_COMPLETE": "诊断采集完成（未进行精度验收）",
|
||||||
"ABORTED": "已中止", "FAILED": "失败", "RUNNING": "标定中",
|
"ABORTED": "已中止", "FAILED": "失败", "RUNNING": "标定中",
|
||||||
"FINALIZING": "拟合与产物校验", "FITTING": "拟合与验证",
|
"FINALIZING": "拟合与产物校验", "FITTING": "拟合与验证",
|
||||||
@@ -130,6 +135,8 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str:
|
|||||||
return "/".join(f"ID{int(item)}" for item in ids) or "无"
|
return "/".join(f"ID{int(item)}" for item in ids) or "无"
|
||||||
phase = str(value.get("phase"))
|
phase = str(value.get("phase"))
|
||||||
phase_label = _PHASES.get(phase, value.get("phase"))
|
phase_label = _PHASES.get(phase, value.get("phase"))
|
||||||
|
if value.get("raw_protocol_progress") and phase == "steady":
|
||||||
|
phase_label = "稳态角点采集"
|
||||||
diagnostic_info = value.get("diagnostic_capture") or {}
|
diagnostic_info = value.get("diagnostic_capture") or {}
|
||||||
diagnostic = bool(diagnostic_info)
|
diagnostic = bool(diagnostic_info)
|
||||||
sample_label = "原始观测" if diagnostic_info.get("coverage_scope") == "raw_observations" else "有效样本"
|
sample_label = "原始观测" if diagnostic_info.get("coverage_scope") == "raw_observations" else "有效样本"
|
||||||
@@ -150,11 +157,17 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str:
|
|||||||
f"[{value.get('model', '未知型号')} / {value.get('side', '?')} / {value.get('serial_number', '?')}] {progress:6.1%} 预计剩余 {eta_text}",
|
f"[{value.get('model', '未知型号')} / {value.get('side', '?')} / {value.get('serial_number', '?')}] {progress:6.1%} 预计剩余 {eta_text}",
|
||||||
stage_line,
|
stage_line,
|
||||||
f"任务：{task.get('label') or task.get('key') or '无'}；方向：{task.get('direction') or '-'}",
|
f"任务：{task.get('label') or task.get('key') or '无'}；方向：{task.get('direction') or '-'}",
|
||||||
f"命令/反馈：{angle(motion.get('command'))}/{angle(motion.get('feedback'))}",
|
f"命令/反馈{'（'+motion['channel']+'）' if motion.get('channel') else ''}："
|
||||||
|
f"{angle(motion.get('command'))}/{angle(motion.get('feedback'))}",
|
||||||
"任务所需 Tag：" + ("；".join(f"{view}={tags(ids)}" for view, ids in task.get("required_tag_ids_by_view", {}).items()) or "无"),
|
"任务所需 Tag：" + ("；".join(f"{view}={tags(ids)}" for view, ids in task.get("required_tag_ids_by_view", {}).items()) or "无"),
|
||||||
f"Tag：已识别 {tags(value.get('recognized_tag_ids', ()))}；缺失 {tags(value.get('missing_tag_ids', ()))}；过滤 {tags(value.get('rejected_tag_ids', ()))}",
|
f"Tag：已识别 {tags(value.get('recognized_tag_ids', ()))}；缺失 {tags(value.get('missing_tag_ids', ()))}；过滤 {tags(value.get('rejected_tag_ids', ()))}",
|
||||||
f"{sample_label} {acquisition.get('valid_samples', 0)}；覆盖 {float(acquisition.get('coverage_01', 0)):.1%}；分箱 {acquisition.get('bins', 0)}；内部空白 {acquisition.get('maximum_gap', 0)}；重扫 {acquisition.get('retry_count', 0)} 次",
|
f"{sample_label} {acquisition.get('valid_samples', 0)}；覆盖 {float(acquisition.get('coverage_01', 0)):.1%}；分箱 {acquisition.get('bins', 0)}；内部空白 {acquisition.get('maximum_gap', 0)}；重扫 {acquisition.get('retry_count', 0)} 次",
|
||||||
]
|
]
|
||||||
|
if task.get('segment_key'):
|
||||||
|
key = task['segment_key']
|
||||||
|
path = '；'.join(f'{channel} {start:g}→{end:g}'
|
||||||
|
for channel, (start, end) in task.get('segment_commands', {}).items())
|
||||||
|
lines.insert(3, f"子段：{_SEGMENTS.get(key, key)}；计划指令：{path} {unit}")
|
||||||
if diagnostic:
|
if diagnostic:
|
||||||
label = diagnostic_capture_label(diagnostic_info.get("mode", "o6_thumb"))
|
label = diagnostic_capture_label(diagnostic_info.get("mode", "o6_thumb"))
|
||||||
lines.insert(1, f"模式：{label}；仅采集观测，不发布标定 JSON/URDF")
|
lines.insert(1, f"模式：{label}；仅采集观测，不发布标定 JSON/URDF")
|
||||||
|
|||||||
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -46,7 +46,7 @@ def synthetic_capture(*, scope='full', retained_offset_rad=0.0, camera_aligned_r
|
|||||||
'ring_mcp_pitch', 'pinky_mcp_pitch', 'index_pip', 'middle_pip', 'ring_pip', 'pinky_pip',
|
'ring_mcp_pitch', 'pinky_mcp_pitch', 'index_pip', 'middle_pip', 'ring_pip', 'pinky_pip',
|
||||||
'thumb_ip', 'index_dip', 'middle_dip', 'ring_dip', 'pinky_dip']
|
'thumb_ip', 'index_dip', 'middle_dip', 'ring_dip', 'pinky_dip']
|
||||||
base = [0, 0, 255, 196, 127, 52] + [0]*14
|
base = [0, 0, 255, 196, 127, 52] + [0]*14
|
||||||
signs = [1, 1, -1, -1, -1, -1] + [1]*14
|
signs = [1]*20 # Confirmed native directions; independent XML FK truth.
|
||||||
mount = rigid((.12, -.09, .2), (.009, -.025, .016))
|
mount = rigid((.12, -.09, .2), (.009, -.025, .016))
|
||||||
hand = rigid((.11, -.08, .17), (.11, -.04, .72))
|
hand = rigid((.11, -.08, .17), (.11, -.04, .72))
|
||||||
base_tag = rigid((.2, -.1, .3), (.003, .008, .055))
|
base_tag = rigid((.2, -.1, .3), (.003, .008, .055))
|
||||||
|
|||||||
@@ -1,432 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.acquisition import (
|
|
||||||
ContinuousSweepCollector,
|
|
||||||
Observation,
|
|
||||||
PointCollector,
|
|
||||||
StateSample,
|
|
||||||
TagQuality,
|
|
||||||
aggregate_observations,
|
|
||||||
aggregate_sweep_observations,
|
|
||||||
interpolate_state_u8,
|
|
||||||
required_resume_views,
|
|
||||||
tag_quality_is_valid,
|
|
||||||
update_pnp_reset_watchdog,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.acquisition import PAIR_NAMES
|
|
||||||
|
|
||||||
|
|
||||||
def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
|
|
||||||
since, reset = update_pnp_reset_watchdog(
|
|
||||||
detection_good=True,
|
|
||||||
pnp_valid=False,
|
|
||||||
now=10.0,
|
|
||||||
invalid_since=None,
|
|
||||||
reset_after_seconds=1.0,
|
|
||||||
)
|
|
||||||
assert since == 10.0
|
|
||||||
assert reset is False
|
|
||||||
|
|
||||||
since, reset = update_pnp_reset_watchdog(
|
|
||||||
detection_good=True,
|
|
||||||
pnp_valid=False,
|
|
||||||
now=11.01,
|
|
||||||
invalid_since=since,
|
|
||||||
reset_after_seconds=1.0,
|
|
||||||
)
|
|
||||||
assert since == 11.01
|
|
||||||
assert reset is True
|
|
||||||
|
|
||||||
|
|
||||||
def test_pnp_watchdog_clears_on_valid_pose_or_bad_detection() -> None:
|
|
||||||
assert update_pnp_reset_watchdog(
|
|
||||||
detection_good=True,
|
|
||||||
pnp_valid=True,
|
|
||||||
now=11.0,
|
|
||||||
invalid_since=10.0,
|
|
||||||
reset_after_seconds=1.0,
|
|
||||||
) == (None, False)
|
|
||||||
assert update_pnp_reset_watchdog(
|
|
||||||
detection_good=False,
|
|
||||||
pnp_valid=False,
|
|
||||||
now=11.0,
|
|
||||||
invalid_since=10.0,
|
|
||||||
reset_after_seconds=1.0,
|
|
||||||
) == (None, False)
|
|
||||||
|
|
||||||
|
|
||||||
def test_resume_requires_only_the_active_view() -> None:
|
|
||||||
assert required_resume_views("top") == ("top",)
|
|
||||||
assert required_resume_views("front") == ("front",)
|
|
||||||
assert required_resume_views(None) == ("front", "side", "top")
|
|
||||||
with pytest.raises(ValueError, match="unknown"):
|
|
||||||
required_resume_views("rear")
|
|
||||||
|
|
||||||
|
|
||||||
def _observation(index: int, angle_rad: float = 0.0) -> Observation:
|
|
||||||
quaternion = tuple(
|
|
||||||
float(value)
|
|
||||||
for value in Rotation.from_rotvec([0.0, angle_rad, 0.0]).as_quat()
|
|
||||||
)
|
|
||||||
return Observation(
|
|
||||||
stamp_ns=index,
|
|
||||||
received_at=index / 30.0,
|
|
||||||
relative_quaternion_xyzw={pair: quaternion for pair in PAIR_NAMES},
|
|
||||||
tag_quality={
|
|
||||||
role: TagQuality(hamming=0, decision_margin=50.0, edge_pixels=45.0)
|
|
||||||
for role in ("t0", "t3", "t4", "t5")
|
|
||||||
},
|
|
||||||
state_u8=tuple(float(value) for value in range(20)),
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _point_observation(
|
|
||||||
index: int,
|
|
||||||
*,
|
|
||||||
angle_rad: float = 0.0,
|
|
||||||
motor_value: float = 100.0,
|
|
||||||
) -> Observation:
|
|
||||||
state = [255.0] * 20
|
|
||||||
state[0] = float(motor_value)
|
|
||||||
return replace(
|
|
||||||
_observation(index, angle_rad),
|
|
||||||
state_u8=tuple(state),
|
|
||||||
state_stamp_ns=index,
|
|
||||||
state_sync_error_ns=5_000_000,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_stable_window_then_thirty_frame_capture() -> None:
|
|
||||||
collector = PointCollector(
|
|
||||||
stable_frames=15,
|
|
||||||
capture_frames=30,
|
|
||||||
minimum_settle_seconds=0.4,
|
|
||||||
maximum_stable_spread_rad=np.deg2rad(0.3),
|
|
||||||
)
|
|
||||||
collector.start(0.0)
|
|
||||||
result = None
|
|
||||||
for index in range(15):
|
|
||||||
result = collector.add(_observation(index), index / 30.0)
|
|
||||||
assert result is None
|
|
||||||
assert collector.state == "capturing"
|
|
||||||
assert collector.stable_frames_seen == 15
|
|
||||||
assert collector.capture_frames_seen == 0
|
|
||||||
for index in range(15, 45):
|
|
||||||
result = collector.add(_observation(index), index / 30.0)
|
|
||||||
assert result is not None
|
|
||||||
assert collector.capture_frames_seen == 30
|
|
||||||
assert result["valid_frames"] == 30
|
|
||||||
assert len(result["state_u8_median"]) == 20
|
|
||||||
|
|
||||||
|
|
||||||
def test_translation_stability_ignores_planar_orientation_jitter() -> None:
|
|
||||||
collector = PointCollector(
|
|
||||||
stable_frames=3,
|
|
||||||
capture_frames=3,
|
|
||||||
minimum_settle_seconds=0.0,
|
|
||||||
stability_mode="translation",
|
|
||||||
maximum_stable_translation_spread_m=0.001,
|
|
||||||
)
|
|
||||||
collector.start(0.0)
|
|
||||||
result = None
|
|
||||||
for index in range(6):
|
|
||||||
observation = replace(
|
|
||||||
_observation(index, angle_rad=np.deg2rad(10.0 * index)),
|
|
||||||
tag_translation_xyz_m={
|
|
||||||
"t0": (0.00, 0.00, 0.50),
|
|
||||||
"t3": (0.03, 0.00, 0.50),
|
|
||||||
"t4": (0.06, 0.00, 0.50),
|
|
||||||
"t5": (0.09, 0.00, 0.50),
|
|
||||||
},
|
|
||||||
)
|
|
||||||
result = collector.add(observation, index / 30.0)
|
|
||||||
assert result is not None
|
|
||||||
assert collector.state == "complete"
|
|
||||||
assert collector.stable_spread_rad == {}
|
|
||||||
assert max(collector.stable_spread_m.values()) == 0.0
|
|
||||||
|
|
||||||
|
|
||||||
def test_point_capture_waits_for_synchronised_target_state() -> None:
|
|
||||||
collector = PointCollector(
|
|
||||||
stable_frames=3,
|
|
||||||
capture_frames=3,
|
|
||||||
minimum_settle_seconds=0.0,
|
|
||||||
maximum_stable_spread_rad=np.deg2rad(0.3),
|
|
||||||
)
|
|
||||||
collector.start(
|
|
||||||
0.0,
|
|
||||||
required_state_index=0,
|
|
||||||
required_state_u8=100.0,
|
|
||||||
maximum_state_error_u8=2.0,
|
|
||||||
)
|
|
||||||
for index in range(3):
|
|
||||||
collector.add(
|
|
||||||
_point_observation(index, motor_value=108.0),
|
|
||||||
index / 30.0,
|
|
||||||
)
|
|
||||||
assert collector.state == "settling"
|
|
||||||
assert collector.stable_frames_seen == 0
|
|
||||||
assert collector.reason == "motor_position_out_of_tolerance"
|
|
||||||
|
|
||||||
result = None
|
|
||||||
for index in range(3, 9):
|
|
||||||
result = collector.add(
|
|
||||||
_point_observation(index, motor_value=101.0),
|
|
||||||
index / 30.0,
|
|
||||||
)
|
|
||||||
assert result is not None
|
|
||||||
assert collector.state == "complete"
|
|
||||||
assert result["state_u8_median"][0] == 101.0
|
|
||||||
|
|
||||||
|
|
||||||
def test_unstable_capture_frames_are_not_aggregated() -> None:
|
|
||||||
collector = PointCollector(
|
|
||||||
stable_frames=3,
|
|
||||||
capture_frames=3,
|
|
||||||
minimum_settle_seconds=0.0,
|
|
||||||
maximum_stable_spread_rad=np.deg2rad(0.3),
|
|
||||||
)
|
|
||||||
collector.start(0.0)
|
|
||||||
for index in range(3):
|
|
||||||
collector.add(_observation(index), index / 30.0)
|
|
||||||
assert collector.state == "capturing"
|
|
||||||
|
|
||||||
result = None
|
|
||||||
# The capture block is internally stable but belongs to a different
|
|
||||||
# planar-PnP branch than the preceding stable window.
|
|
||||||
for index, angle_deg in enumerate((25.0, 25.0, 25.0), start=3):
|
|
||||||
result = collector.add(
|
|
||||||
_observation(index, np.deg2rad(angle_deg)),
|
|
||||||
index / 30.0,
|
|
||||||
)
|
|
||||||
assert result is None
|
|
||||||
assert collector.state == "settling"
|
|
||||||
assert collector.capture_frames_seen == 0
|
|
||||||
assert collector.reason.endswith("capture_not_stable")
|
|
||||||
|
|
||||||
|
|
||||||
def test_position_drift_during_capture_restarts_settling() -> None:
|
|
||||||
collector = PointCollector(
|
|
||||||
stable_frames=3,
|
|
||||||
capture_frames=3,
|
|
||||||
minimum_settle_seconds=0.0,
|
|
||||||
)
|
|
||||||
collector.start(
|
|
||||||
0.0,
|
|
||||||
required_state_index=0,
|
|
||||||
required_state_u8=100.0,
|
|
||||||
maximum_state_error_u8=2.0,
|
|
||||||
)
|
|
||||||
for index in range(3):
|
|
||||||
collector.add(
|
|
||||||
_point_observation(index, motor_value=100.0),
|
|
||||||
index / 30.0,
|
|
||||||
)
|
|
||||||
assert collector.state == "capturing"
|
|
||||||
collector.add(
|
|
||||||
_point_observation(3, motor_value=108.0),
|
|
||||||
0.1,
|
|
||||||
)
|
|
||||||
assert collector.state == "settling"
|
|
||||||
assert collector.capture_frames_seen == 0
|
|
||||||
assert collector.reason == "motor_position_out_of_tolerance"
|
|
||||||
|
|
||||||
|
|
||||||
def test_missing_tags_eventually_pauses_point_collector() -> None:
|
|
||||||
collector = PointCollector(settle_timeout_seconds=5.0)
|
|
||||||
collector.start(10.0)
|
|
||||||
collector.poll(15.01)
|
|
||||||
assert collector.state == "failed"
|
|
||||||
assert collector.reason == "settle_timeout"
|
|
||||||
|
|
||||||
|
|
||||||
def test_unstable_window_does_not_enter_capture() -> None:
|
|
||||||
collector = PointCollector(maximum_stable_spread_rad=np.deg2rad(0.3))
|
|
||||||
collector.start(0.0)
|
|
||||||
for index in range(15):
|
|
||||||
angle = np.deg2rad(1.0 if index % 2 else -1.0)
|
|
||||||
collector.add(_observation(index, angle), index / 30.0)
|
|
||||||
assert collector.state == "settling"
|
|
||||||
assert collector.reason.endswith("not_stable")
|
|
||||||
assert max(collector.stable_spread_rad.values()) > np.deg2rad(0.3)
|
|
||||||
|
|
||||||
|
|
||||||
def test_isolated_invalid_frame_is_skipped_without_losing_valid_window() -> None:
|
|
||||||
collector = PointCollector()
|
|
||||||
collector.start(0.0)
|
|
||||||
for index in range(14):
|
|
||||||
collector.add(_observation(index), index / 30.0)
|
|
||||||
collector.mark_invalid_frame()
|
|
||||||
assert collector.stable_frames_seen == 14
|
|
||||||
collector.add(_observation(15), 0.5)
|
|
||||||
assert collector.state == "capturing"
|
|
||||||
assert collector.reason == ""
|
|
||||||
|
|
||||||
|
|
||||||
def test_three_consecutive_invalid_frames_clear_stability_window() -> None:
|
|
||||||
collector = PointCollector()
|
|
||||||
collector.start(0.0)
|
|
||||||
for index in range(14):
|
|
||||||
collector.add(_observation(index), index / 30.0)
|
|
||||||
for _ in range(3):
|
|
||||||
collector.mark_invalid_frame()
|
|
||||||
assert collector.stable_frames_seen == 0
|
|
||||||
collector.add(_observation(15), 0.5)
|
|
||||||
assert collector.state == "settling"
|
|
||||||
|
|
||||||
|
|
||||||
def test_bad_tag_quality_is_filtered() -> None:
|
|
||||||
good = TagQuality(hamming=0, decision_margin=31.0, edge_pixels=40.0)
|
|
||||||
bad_hamming = TagQuality(hamming=1, decision_margin=50.0, edge_pixels=50.0)
|
|
||||||
thresholds = {
|
|
||||||
"maximum_hamming": 0,
|
|
||||||
"minimum_decision_margin": 30.0,
|
|
||||||
"minimum_edge_pixels": 40.0,
|
|
||||||
}
|
|
||||||
assert tag_quality_is_valid(good, **thresholds)
|
|
||||||
assert not tag_quality_is_valid(bad_hamming, **thresholds)
|
|
||||||
|
|
||||||
|
|
||||||
def test_aggregate_keeps_worst_tag_quality() -> None:
|
|
||||||
observations = [_observation(0), _observation(1)]
|
|
||||||
aggregate = aggregate_observations(observations)
|
|
||||||
assert aggregate["tag_quality"]["t0"] == {
|
|
||||||
"minimum_decision_margin": 50.0,
|
|
||||||
"minimum_edge_pixels": 45.0,
|
|
||||||
"maximum_hamming": 0,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def test_aggregate_keeps_median_tag_centres() -> None:
|
|
||||||
observations = [
|
|
||||||
replace(
|
|
||||||
_observation(index),
|
|
||||||
tag_translation_xyz_m={
|
|
||||||
role: (0.01 * index, 0.02, 0.50)
|
|
||||||
for role in ("t0", "t3", "t4", "t5")
|
|
||||||
},
|
|
||||||
)
|
|
||||||
for index in range(3)
|
|
||||||
]
|
|
||||||
aggregate = aggregate_observations(observations)
|
|
||||||
assert aggregate["tag_translation_xyz_m"]["t4"] == pytest.approx(
|
|
||||||
[0.01, 0.02, 0.50]
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_pnp_reprojection_error_is_filtered_and_aggregated() -> None:
|
|
||||||
good = TagQuality(
|
|
||||||
hamming=0,
|
|
||||||
decision_margin=50.0,
|
|
||||||
edge_pixels=40.0,
|
|
||||||
reprojection_error_px=0.4,
|
|
||||||
)
|
|
||||||
bad = replace(good, reprojection_error_px=1.6)
|
|
||||||
thresholds = {
|
|
||||||
"maximum_hamming": 0,
|
|
||||||
"minimum_decision_margin": 30.0,
|
|
||||||
"minimum_edge_pixels": 30.0,
|
|
||||||
"maximum_reprojection_error_px": 1.5,
|
|
||||||
}
|
|
||||||
assert tag_quality_is_valid(good, **thresholds)
|
|
||||||
assert not tag_quality_is_valid(bad, **thresholds)
|
|
||||||
|
|
||||||
observations = [
|
|
||||||
replace(
|
|
||||||
_observation(index),
|
|
||||||
tag_quality={
|
|
||||||
role: replace(good, reprojection_error_px=error)
|
|
||||||
for role in ("t0", "t3", "t4", "t5")
|
|
||||||
},
|
|
||||||
)
|
|
||||||
for index, error in enumerate((0.2, 0.7))
|
|
||||||
]
|
|
||||||
aggregate = aggregate_observations(observations)
|
|
||||||
assert (
|
|
||||||
aggregate["tag_quality"]["t0"][
|
|
||||||
"maximum_reprojection_error_px"
|
|
||||||
]
|
|
||||||
== 0.7
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_state_is_interpolated_at_camera_timestamp() -> None:
|
|
||||||
before = tuple([255.0] + [0.0] * 19)
|
|
||||||
after = tuple([235.0] + [0.0] * 19)
|
|
||||||
samples = [
|
|
||||||
StateSample(stamp_ns=1_000_000_000, position_u8=before),
|
|
||||||
StateSample(stamp_ns=1_100_000_000, position_u8=after),
|
|
||||||
]
|
|
||||||
matched = interpolate_state_u8(
|
|
||||||
samples,
|
|
||||||
1_025_000_000,
|
|
||||||
maximum_skew_ns=60_000_000,
|
|
||||||
)
|
|
||||||
assert matched is not None
|
|
||||||
state, skew = matched
|
|
||||||
assert state[0] == 250.0
|
|
||||||
assert skew == 25_000_000
|
|
||||||
assert (
|
|
||||||
interpolate_state_u8(
|
|
||||||
samples,
|
|
||||||
1_300_000_000,
|
|
||||||
maximum_skew_ns=60_000_000,
|
|
||||||
)
|
|
||||||
is None
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _synchronised_observation(
|
|
||||||
index: int, motor_value: float
|
|
||||||
) -> Observation:
|
|
||||||
state = [255.0] * 20
|
|
||||||
state[0] = motor_value
|
|
||||||
return replace(
|
|
||||||
_observation(index),
|
|
||||||
state_u8=tuple(state),
|
|
||||||
state_stamp_ns=index,
|
|
||||||
state_sync_error_ns=5_000_000,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_continuous_sweep_completes_after_full_span_and_endpoint_hold() -> None:
|
|
||||||
collector = ContinuousSweepCollector(
|
|
||||||
endpoint_hold_seconds=0.2,
|
|
||||||
minimum_valid_frames=20,
|
|
||||||
minimum_state_span_u8=240.0,
|
|
||||||
)
|
|
||||||
collector.start(0.0, motor_index=0, start_u8=255, target_u8=0)
|
|
||||||
result = None
|
|
||||||
for index, value in enumerate(np.linspace(255.0, 0.0, 60)):
|
|
||||||
result = collector.add(
|
|
||||||
_synchronised_observation(index, float(value)),
|
|
||||||
index * 0.05,
|
|
||||||
)
|
|
||||||
assert result is None
|
|
||||||
for offset in range(1, 6):
|
|
||||||
result = collector.add(
|
|
||||||
_synchronised_observation(60 + offset, 0.0),
|
|
||||||
3.0 + offset * 0.05,
|
|
||||||
)
|
|
||||||
if result is not None:
|
|
||||||
break
|
|
||||||
assert result is not None
|
|
||||||
assert collector.state == "complete"
|
|
||||||
assert collector.state_span_u8 == 255.0
|
|
||||||
|
|
||||||
bins = aggregate_sweep_observations(
|
|
||||||
result,
|
|
||||||
motor_index=0,
|
|
||||||
start_u8=255,
|
|
||||||
target_u8=0,
|
|
||||||
endpoint_tolerance_u8=2.0,
|
|
||||||
)
|
|
||||||
assert 0 in bins
|
|
||||||
assert 255 in bins
|
|
||||||
assert len(bins) >= 50
|
|
||||||
@@ -1,109 +0,0 @@
|
|||||||
"""A training plan saves stops without reading the independent validation set."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.profile import observation_streams
|
|
||||||
from linkerhand_calibration.core.domain.sampling import command_nodes
|
|
||||||
from linkerhand_calibration.core.fitting.command_sampling import (
|
|
||||||
build_sampling_plan, declared_training_nodes, reduced_knots, validate_sampling_plans,
|
|
||||||
MAXIMUM_REMOVAL_ERROR_RAD,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.product import load_product_config
|
|
||||||
|
|
||||||
|
|
||||||
def source_rows(profile, task, *, curved=False):
|
|
||||||
rows = []
|
|
||||||
for cycle in range(4):
|
|
||||||
for direction in ("increasing", "decreasing"):
|
|
||||||
for i, x in enumerate(np.linspace(task.start_value, task.end_value, 81)):
|
|
||||||
phase = (x-min(task.start_value, task.end_value))/abs(task.end_value-task.start_value)
|
|
||||||
angle = .8*phase + (.35*phase**3 if curved else 0)
|
|
||||||
for field, name, spec in observation_streams(profile, task):
|
|
||||||
rows.append({"kind": "joint_sample", field: name, "view": spec.view,
|
|
||||||
"task_name": task.key, "image_stamp_ns": 1000000+len(rows), "cycle": cycle,
|
|
||||||
"direction": direction, "sample_phase": "sweep", "attempt": 1,
|
|
||||||
f"feedback_{profile.command.unit}": float(x),
|
|
||||||
"relative_quaternion_xyzw": Rotation.from_rotvec([0, 0, angle]).as_quat().tolist()})
|
|
||||||
return rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("name", ["g20", "l6", "o6", "o12"])
|
|
||||||
def test_all_models_reduce_linear_training_and_keep_validation_unchanged(name):
|
|
||||||
profile = load_product_config(Path(__file__).resolve().parents[1]/f"config/{name}_right_product.yaml",
|
|
||||||
check_can=False).calibration_contract.typed_profile
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
rows = source_rows(profile, task)
|
|
||||||
plan = build_sampling_plan(profile, task, rows)
|
|
||||||
assert 3 <= len(plan["training_nodes"]) <= 4
|
|
||||||
assert len(plan["training_nodes"]) < len(command_nodes(profile, task))
|
|
||||||
assert tuple(plan["validation_nodes"]) == command_nodes(profile, task, holdout=True)
|
|
||||||
assert validate_sampling_plans(profile, [*rows, plan]) == {task.key: plan}
|
|
||||||
changed = deepcopy(rows)
|
|
||||||
for row in changed:
|
|
||||||
if row["cycle"] == 3:
|
|
||||||
row["relative_quaternion_xyzw"] = [1, 0, 0, 0]
|
|
||||||
row[f"feedback_{profile.command.unit}"] += 100
|
|
||||||
assert build_sampling_plan(profile, task, changed) == plan
|
|
||||||
assert len(build_sampling_plan(profile, task, source_rows(profile, task, curved=True))["training_nodes"]) > 3
|
|
||||||
missing = [row for row in rows if row["cycle"] != 1]
|
|
||||||
assert tuple(build_sampling_plan(profile, task, missing)["training_nodes"]) == command_nodes(profile, task)
|
|
||||||
changed[0]["relative_quaternion_xyzw"] = [0, 0, .1, .99498743710662]
|
|
||||||
with pytest.raises(ValueError, match="sampling_training_evidence_changed"):
|
|
||||||
validate_sampling_plans(profile, [*changed, plan])
|
|
||||||
damaged = {**plan, "training_nodes": [0, 255]}
|
|
||||||
with pytest.raises(ValueError, match="sampling_plan_hash_changed"):
|
|
||||||
declared_training_nodes(profile, task, [damaged])
|
|
||||||
|
|
||||||
|
|
||||||
def test_command_release_selects_knots_without_a_feedback_angle_fit():
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
profile = load_bundled_hand_profile("o30_right_18", scope="full")
|
|
||||||
assert not profile.vision_motion and profile.command_based_release
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
rows = source_rows(profile, task, curved=True)
|
|
||||||
for row in rows:
|
|
||||||
row["command_u8"] = row["feedback_u8"]
|
|
||||||
row["feedback_u8"] = min(255., row["feedback_u8"]+10.)
|
|
||||||
plan = build_sampling_plan(profile, task, rows)
|
|
||||||
assert plan["source_count"] > 0
|
|
||||||
changed = [dict(row, feedback_u8=0.) for row in rows]
|
|
||||||
assert build_sampling_plan(profile, task, changed) == plan
|
|
||||||
|
|
||||||
|
|
||||||
def test_shortest_plan_respects_each_branch_and_mandatory_nodes():
|
|
||||||
grid = np.linspace(0, 1, 9)
|
|
||||||
curves = np.array([grid, grid + .03*np.sin(6*grid), grid**2])
|
|
||||||
selected = reduced_knots(grid, curves, {0, 4, 8})
|
|
||||||
assert {0, .5, 1} <= set(selected)
|
|
||||||
for curve in curves:
|
|
||||||
actual = np.interp(grid, selected, np.interp(selected, grid, curve))
|
|
||||||
assert max(abs(actual-curve)) <= MAXIMUM_REMOVAL_ERROR_RAD
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["l6_right_8", "o12_right_16"])
|
|
||||||
def test_reduced_static_training_still_rejects_wrong_independent_validation(layout):
|
|
||||||
from dataclasses import replace
|
|
||||||
from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings
|
|
||||||
from test_dual_mapping import independent_steady_fixture
|
|
||||||
profile, fit, name, rows, _ = independent_steady_fixture(layout)
|
|
||||||
profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate"))
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
plan = build_sampling_plan(profile, task, source_rows(profile, task))
|
|
||||||
assert len(plan["training_nodes"]) < len(command_nodes(profile, task))
|
|
||||||
selected = [dict(row, cycle={0: 4, 3: 5}[row["cycle"]],
|
|
||||||
steady_training_nodes=plan["training_nodes"], sampling_plan_sha256=plan["plan_sha256"])
|
|
||||||
for row in rows if row["cycle"] == 3
|
|
||||||
or (row["cycle"] == 0 and row["steady_target"] in plan["training_nodes"])]
|
|
||||||
assert fit_command_mappings(profile, fit, {name: selected}).command_mappings
|
|
||||||
for row in selected:
|
|
||||||
if row["cycle"] == 5:
|
|
||||||
row["relative_quaternion_xyzw"] = (Rotation.from_rotvec([0, 0, .09])
|
|
||||||
* Rotation.from_quat(row["relative_quaternion_xyzw"])).as_quat()
|
|
||||||
with pytest.raises(ValueError, match="steady_command_holdout_failed"):
|
|
||||||
fit_command_mappings(profile, fit, {name: selected})
|
|
||||||
assert not fit.command_mappings
|
|
||||||
@@ -1,159 +0,0 @@
|
|||||||
"""Training decisions use real independent cycles and never held-out images."""
|
|
||||||
|
|
||||||
import math
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import ADAPTIVE, configure_training, select_task_training
|
|
||||||
from linkerhand_calibration.core.domain.reference import read_joint_zero_references
|
|
||||||
from linkerhand_calibration.core.fitting.command_motion import fit_command_training
|
|
||||||
from linkerhand_calibration.core.fitting.training_quality import assess_training, training_snapshot
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(scope="module")
|
|
||||||
def training(o30_capture):
|
|
||||||
profile, source, records, _ = o30_capture
|
|
||||||
profile = configure_training(profile, ADAPTIVE)
|
|
||||||
return profile, source, records, read_joint_zero_references(profile, records)
|
|
||||||
|
|
||||||
|
|
||||||
def local_records(training):
|
|
||||||
profile, _, records, references = training
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
return task, [r for r in records if r.get("task_name") == task.key], {
|
|
||||||
name: references[name] for name in task.joints}
|
|
||||||
|
|
||||||
|
|
||||||
def test_missing_future_geometry_retains_third_round_and_freezes_once(training):
|
|
||||||
profile, source, _, _ = training
|
|
||||||
task, rows, references = local_records(training)
|
|
||||||
for cycles, expected in [((0, 1), "add_training"), ((0, 1, 2), "freeze")]:
|
|
||||||
snapshot = training_snapshot(profile, rows, task.key, cycles)
|
|
||||||
result = assess_training(profile, task.key, cycles, snapshot, references, source)
|
|
||||||
assert result["decision"] == expected, result
|
|
||||||
assert result["geometry"]["status"] == "deferred"
|
|
||||||
assert not result["failures"]
|
|
||||||
|
|
||||||
|
|
||||||
def test_persistent_cycle_disagreement_fails_after_third_round(training):
|
|
||||||
profile, source, _, _ = training
|
|
||||||
task, rows, references = local_records(training)
|
|
||||||
snapshot = training_snapshot(profile, rows, task.key, (0, 1, 2))
|
|
||||||
# Rotate a whole independent cycle, keeping coverage and image count fixed.
|
|
||||||
axis = (Rotation.from_quat(snapshot[20]["relative_quaternion_xyzw"]).inv()
|
|
||||||
* Rotation.from_quat(snapshot[50]["relative_quaternion_xyzw"])).as_rotvec()
|
|
||||||
axis /= np.linalg.norm(axis)
|
|
||||||
changed = [dict(row, relative_quaternion_xyzw=(Rotation.from_quat(row["relative_quaternion_xyzw"])
|
|
||||||
* Rotation.from_rotvec(axis * (.06 if row["cycle"] == 1 else 0.))).as_quat().tolist()) for row in snapshot]
|
|
||||||
for cycles, expected in [((0, 1), "add_training"), ((0, 1, 2), "fail")]:
|
|
||||||
result = assess_training(profile, task.key, cycles,
|
|
||||||
tuple(r for r in changed if r["cycle"] in cycles), references, source)
|
|
||||||
assert result["decision"] == expected
|
|
||||||
assert result["failures"]
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_snapshot_excludes_poisoned_validation_and_rejected_attempts(training):
|
|
||||||
profile, _, _, _ = training
|
|
||||||
task, rows, _ = local_records(training)
|
|
||||||
first = training_snapshot(profile, rows, task.key, (0, 1))
|
|
||||||
poisoned = [dict(r, relative_quaternion_xyzw=[float("nan")]*4) if r.get("cycle") == 3 else r for r in rows]
|
|
||||||
assert training_snapshot(profile, poisoned, task.key, (0, 1)) == first
|
|
||||||
rejected = dict(first[0], kind="pnp_candidate_frame", attempt=2)
|
|
||||||
latest = training_snapshot(profile, [*rows, rejected], task.key, (0, 1))
|
|
||||||
assert len(latest) < len(first)
|
|
||||||
assert all(r["cycle"] != 3 for r in latest)
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_interfaces_reject_holdout_before_fitting(training):
|
|
||||||
profile, source, _, _ = training
|
|
||||||
task, rows, references = local_records(training)
|
|
||||||
sweep = {name: [r for r in rows if r.get("joint") == name and r.get("sample_phase") == "sweep"]
|
|
||||||
for name in task.joints}
|
|
||||||
steady = {name: [r for r in rows if r.get("joint") == name and r.get("sample_phase") == "steady"]
|
|
||||||
for name in task.joints}
|
|
||||||
with pytest.raises(ValueError, match="motion_training_received_nontraining_data"):
|
|
||||||
fit_command_training(profile, UrdfKinematicModel(source), sweep, steady, references)
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_worker_timeout_and_close_leave_no_process(training):
|
|
||||||
import multiprocessing
|
|
||||||
import time
|
|
||||||
from linkerhand_calibration.runtime.training import TrainingWorker
|
|
||||||
profile, source, _, _ = training
|
|
||||||
task, rows, references = local_records(training)
|
|
||||||
worker = TrainingWorker(timeout_seconds=0.)
|
|
||||||
request = dict(profile=profile, source_urdf=source, task_key=task.key, cycles=(0, 1),
|
|
||||||
rows=training_snapshot(profile, rows, task.key, (0, 1)), references=references)
|
|
||||||
try:
|
|
||||||
deadline = time.monotonic()+10.
|
|
||||||
with pytest.raises(TimeoutError, match="training_assessment_timeout"):
|
|
||||||
while time.monotonic() < deadline:
|
|
||||||
assert worker.poll(**request) is None
|
|
||||||
time.sleep(.01)
|
|
||||||
assert not multiprocessing.active_children()
|
|
||||||
finally:
|
|
||||||
worker.close()
|
|
||||||
with pytest.raises(RuntimeError, match="cancelled"):
|
|
||||||
worker.poll(**request)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.integration
|
|
||||||
@pytest.mark.parametrize("cancel", [False, True])
|
|
||||||
def test_coordinator_keeps_feedback_live_while_training_and_honors_abort(training, tmp_path, cancel):
|
|
||||||
import json
|
|
||||||
import time
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
task, rows, references = local_records(training)
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model="o30",
|
|
||||||
profile_transform=lambda p: configure_training(p, ADAPTIVE))
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
session = host.execution.session
|
|
||||||
session._unit_index = max(i for i, unit in enumerate(session._units)
|
|
||||||
if unit.task_key == task.key and unit.cycle == 1)
|
|
||||||
session.phase = Phase.EVALUATE
|
|
||||||
host.execution.completed_units.update(u.identity for u in session._units[:session._unit_index])
|
|
||||||
host.execution.zero_references.update(references)
|
|
||||||
host.capture_index.extend(r for r in rows if r.get("cycle") in {0, 1})
|
|
||||||
host.last_command = list(host.profile.command.baseline_values)
|
|
||||||
deadline = time.monotonic()+10.
|
|
||||||
while session.phase == Phase.EVALUATE and time.monotonic() < deadline:
|
|
||||||
clock.now += .02
|
|
||||||
host.receive_feedback(host.profile.command.names, host.last_command, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
assert host.feedback_fresh()
|
|
||||||
if cancel:
|
|
||||||
assert host.abort().success
|
|
||||||
time.sleep(.01)
|
|
||||||
saved = [json.loads(line) for line in host.raw_path.read_text().splitlines()]
|
|
||||||
decisions = [row for row in saved if row.get("kind") == "training_decision"]
|
|
||||||
if cancel:
|
|
||||||
assert session.phase == Phase.ABORTED
|
|
||||||
assert not decisions
|
|
||||||
else:
|
|
||||||
assert session.phase == Phase.PREPARE, host.reason
|
|
||||||
assert session.current_unit.cycle == 2
|
|
||||||
assert len(decisions) == 1 and decisions[0]["decision"] == "add_training"
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.integration
|
|
||||||
def test_mixed_cycles_have_real_zero_confidence_and_no_fabricated_third_observation(training):
|
|
||||||
profile, source, rows, references = training
|
|
||||||
profile = select_task_training(profile, "pinky_dip_side", (0, 1))
|
|
||||||
task = profile.motion.tasks[-1]
|
|
||||||
result = assess_training(profile, task.key, (0, 1),
|
|
||||||
training_snapshot(profile, rows, task.key, (0, 1)), references, source)
|
|
||||||
assert result["decision"] == "freeze", result
|
|
||||||
assert result["geometry"]["status"] == "checked"
|
|
||||||
counts = result["geometry"]["independent_cycles_by_joint"]
|
|
||||||
assert counts["pinky_pip"] == 2
|
|
||||||
assert counts["index_pip"] == 3
|
|
||||||
assert all(width is not None and width <= math.radians(1.)
|
|
||||||
for width in result["geometry"]["zero_confidence_half_width_rad"].values())
|
|
||||||
@@ -1,103 +0,0 @@
|
|||||||
"""Keep standalone URDF coupling while JSON retains the measured nonlinear path."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
import json
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from test_independent_urdf_export import nonlinear_release, assert_robot_state_publisher_angles
|
|
||||||
from linkerhand_calibration.core.fitting.coupling import fit_range_bounded_mimic
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan
|
|
||||||
from linkerhand_calibration.runtime.artifacts.urdf_from_json import (
|
|
||||||
rebuild_urdf_from_json, serialize_urdf_correction, validate_json_urdf_correspondence,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def affine_release(nonlinear_release):
|
|
||||||
source, rights, original_plan, payload, path = nonlinear_release
|
|
||||||
model = UrdfKinematicModel(source)
|
|
||||||
parent, child = model.joints["pitch"], model.joints["ip"]
|
|
||||||
x = np.tile(payload["joints"]["pitch"]["angle_rad"], 3)
|
|
||||||
y = np.tile(payload["joints"]["ip"]["angle_rad"], 3)
|
|
||||||
fitted = fit_range_bounded_mimic("pitch", "ip", x, y, cycles=np.repeat([0, 1, 2], 256),
|
|
||||||
parent_range=(parent.lower, parent.upper), child_range=(child.lower, child.upper))
|
|
||||||
plan = build_standard_correction_plan(source_urdf=source,
|
|
||||||
source_sha256=original_plan.source_sha256, zero_offsets_rad={}, authorized_fields=rights,
|
|
||||||
fitted_mimics_output={"ip": fitted})
|
|
||||||
payload = deepcopy(payload)
|
|
||||||
payload["urdf_correction"] = serialize_urdf_correction(plan)
|
|
||||||
path.write_text(json.dumps(payload))
|
|
||||||
return source, rights, plan, payload, path
|
|
||||||
|
|
||||||
|
|
||||||
def test_near_zero_limits_do_not_flatten_affine_motion_and_json_stays_nonlinear(affine_release, tmp_path):
|
|
||||||
source, rights, plan, payload, path = affine_release
|
|
||||||
output = rebuild_urdf_from_json(path, source, tmp_path/"affine.urdf",
|
|
||||||
authorized_fields=rights, expected_profile_id=payload["profile_id"])
|
|
||||||
validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"])
|
|
||||||
assert output.read_bytes() == plan.write(source, tmp_path/"direct.urdf").read_bytes()
|
|
||||||
assert payload["urdf_correction"]["schema_version"] == 3
|
|
||||||
original, model = UrdfKinematicModel(source), UrdfKinematicModel(output)
|
|
||||||
passive = model.joints["ip"]
|
|
||||||
assert passive.mimic_joint == "pitch"
|
|
||||||
assert passive.mimic_multiplier > 1.75
|
|
||||||
assert 0 < passive.mimic_offset < .002
|
|
||||||
for name, joint in original.joints.items():
|
|
||||||
assert (model.joints[name].lower, model.joints[name].upper) == (joint.lower, joint.upper)
|
|
||||||
np.testing.assert_array_equal(model.joints[name].origin, joint.origin)
|
|
||||||
for command in (0, 32, 128, 255):
|
|
||||||
explicit = {name: row["angle_rad"][command] for name, row in payload["joints"].items()}
|
|
||||||
assert model.resolve_angles(explicit, independent_mimic_angles=True) == explicit
|
|
||||||
preview = model.resolve_angles({"pitch": explicit["pitch"]})
|
|
||||||
assert preview["ip"] == pytest.approx(passive.mimic_multiplier*explicit["pitch"]+passive.mimic_offset)
|
|
||||||
assert abs(preview["ip"]-explicit["ip"]) < np.deg2rad(4)
|
|
||||||
for angle in np.linspace(model.joints["pitch"].lower, model.joints["pitch"].upper, 257):
|
|
||||||
value = model.resolve_angles({"pitch": angle})["ip"]
|
|
||||||
assert passive.lower-1e-12 <= value <= passive.upper+1e-12
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("policy", ["missing", "unknown", "legacy"])
|
|
||||||
def test_nonzero_approximation_requires_explicit_serialized_policy(affine_release, tmp_path, policy):
|
|
||||||
source, rights, _, payload, path = affine_release
|
|
||||||
payload = deepcopy(payload)
|
|
||||||
correction = payload["urdf_correction"]
|
|
||||||
if policy == "missing":
|
|
||||||
del correction["mimic_policy"]
|
|
||||||
elif policy == "unknown":
|
|
||||||
correction["mimic_policy"] = "assumed_physical_zero"
|
|
||||||
else:
|
|
||||||
correction["schema_version"] = 1
|
|
||||||
del correction["mimic_policy"], correction["joint_angle_source"]
|
|
||||||
path.write_text(json.dumps(payload))
|
|
||||||
output = tmp_path/"invalid.urdf"
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
rebuild_urdf_from_json(path, source, output, authorized_fields=rights,
|
|
||||||
expected_profile_id=payload["profile_id"])
|
|
||||||
assert not output.exists()
|
|
||||||
|
|
||||||
|
|
||||||
def test_standard_ros_parent_input_drives_passive_joint(affine_release, tmp_path):
|
|
||||||
assert_robot_state_publisher_angles(affine_release, tmp_path, use_mimic=True)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("parent_range,child_range", [((-.002, .6), (-.003, 1.2)), ((-.6, .002), (-.003, 1.2))])
|
|
||||||
def test_range_bounded_fit_reports_real_residuals_without_rewriting_training_data(parent_range, child_range):
|
|
||||||
sign = 1 if parent_range[1] == .6 else -1
|
|
||||||
x = sign*np.tile(np.linspace(0, .6, 80), 3)
|
|
||||||
y = 1.71*np.abs(x)+.39*x*x
|
|
||||||
original = y.copy()
|
|
||||||
cycles = np.repeat([0, 1, 2], 80)
|
|
||||||
fit = fit_range_bounded_mimic("parent", "child", x, y, cycles=cycles,
|
|
||||||
parent_range=parent_range, child_range=child_range)
|
|
||||||
assert sign*fit.multiplier > 1.7
|
|
||||||
assert fit.offset_observed is False # The approximation intercept is not a physical datum.
|
|
||||||
np.testing.assert_array_equal(y, original)
|
|
||||||
np.testing.assert_allclose(fit.residuals_rad, fit.multiplier*x+fit.offset_rad-y)
|
|
||||||
for endpoint in parent_range:
|
|
||||||
assert child_range[0]-1e-10 <= fit.multiplier*endpoint+fit.offset_rad <= child_range[1]+1e-10
|
|
||||||
with pytest.raises(ValueError, match="holdout is forbidden"):
|
|
||||||
fit_range_bounded_mimic("parent", "child", x, y, cycles=np.repeat([0, 1, 3], 80),
|
|
||||||
parent_range=parent_range, child_range=child_range)
|
|
||||||
@@ -1,39 +0,0 @@
|
|||||||
"""Tests for the independent physical-line alignment overlay."""
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.alignment_view import (
|
|
||||||
summarize_alignment_measurements,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _measurement(angle: float, offset: float, y: float) -> dict:
|
|
||||||
return {
|
|
||||||
"line_xyxy_px": [0.0, y, 100.0, y - angle * 100.0],
|
|
||||||
"angle_rad": angle,
|
|
||||||
"vertical_offset_px": offset,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def test_line_summary_smooths_only_physical_line_measurements() -> None:
|
|
||||||
"""The overlay smooths scene lines without any Tag orientation input."""
|
|
||||||
result = summarize_alignment_measurements(
|
|
||||||
[
|
|
||||||
_measurement(-0.02, -4.0, 80.0),
|
|
||||||
None,
|
|
||||||
_measurement(0.00, 0.0, 82.0),
|
|
||||||
_measurement(0.02, 4.0, 84.0),
|
|
||||||
]
|
|
||||||
)
|
|
||||||
|
|
||||||
assert result is not None
|
|
||||||
assert result["angle_rad"] == pytest.approx(0.0)
|
|
||||||
assert result["vertical_offset_px"] == pytest.approx(0.0)
|
|
||||||
assert result["line_xyxy_px"] == pytest.approx([0.0, 82.0, 100.0, 82.0])
|
|
||||||
assert result["detected_frames"] == 3
|
|
||||||
assert result["window_frames"] == 4
|
|
||||||
|
|
||||||
|
|
||||||
def test_line_summary_returns_none_without_scene_line() -> None:
|
|
||||||
"""No blue line is fabricated when the scene has no valid long edge."""
|
|
||||||
assert summarize_alignment_measurements([None, None]) is None
|
|
||||||
@@ -1,99 +0,0 @@
|
|||||||
"""Shared v3 generation for every product using independent synthetic poses.
|
|
||||||
|
|
||||||
Synthetic geometry/limits are explicit fixture truth, not hardware evidence.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from measured_capture_fixture import measured_capture
|
|
||||||
from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session
|
|
||||||
from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper
|
|
||||||
from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"])
|
|
||||||
def test_separate_motion_mapping_json_urdf_and_reader(layout, tmp_path, monkeypatch):
|
|
||||||
from linkerhand_calibration.runtime.artifacts import finalization
|
|
||||||
from linkerhand_calibration.core.fitting import mimic_approximation
|
|
||||||
from linkerhand_calibration.core.urdf.source_mimic import validate_source_mimic_elements
|
|
||||||
def forbidden_fit(*args, **kwargs):
|
|
||||||
pytest.fail("formal measured calibration must not optimize mimic parameters")
|
|
||||||
monkeypatch.setattr(mimic_approximation, "fit_mimic_approximations", forbidden_fit)
|
|
||||||
monkeypatch.setattr(mimic_approximation, "fit_bounded_mimic_approximations", forbidden_fit)
|
|
||||||
validators = []
|
|
||||||
original_validator = finalization.FrozenTagArtifactValidator
|
|
||||||
def record_validator(*args, **kwargs):
|
|
||||||
from test_final_image_holdout import independent_pixels
|
|
||||||
validator = original_validator(*args, **kwargs)
|
|
||||||
validator = replace(validator, require_image_holdout=True,
|
|
||||||
image_observations=independent_pixels(validator.observations),
|
|
||||||
command_image_observations=independent_pixels(validator.command_observations))
|
|
||||||
validators.append(validator)
|
|
||||||
return validator
|
|
||||||
monkeypatch.setattr(finalization, "FrozenTagArtifactValidator", record_validator)
|
|
||||||
profile, source, records, _ = measured_capture(tmp_path, layout=layout,
|
|
||||||
nonlinear=layout != "o12_right_16")
|
|
||||||
if layout == "o12_right_16":
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
assumptions = load_bundled_hand_profile(layout).zero.cad_zero_assumptions
|
|
||||||
profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions=assumptions,
|
|
||||||
known_baseline_geometry={name: value for name, value in profile.zero.known_baseline_geometry.items()
|
|
||||||
if name not in assumptions}))
|
|
||||||
profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate"))
|
|
||||||
records = [dict(row, cycle={0: 4, 3: 5}[row["cycle"]]) if row.get("sample_phase") == "steady" else row
|
|
||||||
for row in records if row.get("sample_phase") != "steady" or row["cycle"] in {0, 3}]
|
|
||||||
from linkerhand_calibration.core.fitting.command_sampling import build_sampling_plan
|
|
||||||
plans = {task.key: build_sampling_plan(profile, task, records) for task in profile.motion.tasks}
|
|
||||||
records = [dict(row, steady_training_nodes=plans[row["task_name"]]["training_nodes"],
|
|
||||||
sampling_plan_sha256=plans[row["task_name"]]["plan_sha256"])
|
|
||||||
if row.get("sample_phase") == "steady" else row for row in records
|
|
||||||
if row.get("sample_phase") != "steady" or row["cycle"] != 4
|
|
||||||
or row["steady_target"] in plans[row["task_name"]]["training_nodes"]]
|
|
||||||
records.extend(plans.values())
|
|
||||||
hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields}
|
|
||||||
hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest()
|
|
||||||
phases = []
|
|
||||||
payload, fit, result = finalize_profile_session(profile=profile, source_urdf=source,
|
|
||||||
records=records, protected_inputs=hashes, serial_number="SYNTHETIC_ONLY", session_dir=tmp_path/"session",
|
|
||||||
standard_loader=lambda _: None, phase_changed=phases.append)
|
|
||||||
assert phases == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"]
|
|
||||||
assert payload["schema_version"] == 3
|
|
||||||
assert payload["urdf_correction"]["schema_version"] == 4
|
|
||||||
assert payload["urdf_correction"]["mimic_policy"] == "preserve_source_mimic"
|
|
||||||
assert not fit.mimic_approximations
|
|
||||||
validate_source_mimic_elements(source, result.path)
|
|
||||||
assert all(row["training_cycles"] == [4] and row["holdout_cycle"] == 5
|
|
||||||
for row in fit.command_holdout_metrics.values())
|
|
||||||
assert payload["urdf_correction"]["source_urdf_sha256"] == hashes["source_urdf_sha256"]
|
|
||||||
accepted = json.loads(result.staged_release.validation_json)
|
|
||||||
assert accepted["command_holdout_cycle"] == 5
|
|
||||||
assert accepted["final_file_image_holdout_verified"] is True
|
|
||||||
assert accepted["final_file_image_holdout_policy"] == "independent_measurement_and_physical_consistency_v2"
|
|
||||||
assert set(accepted["final_file_image_holdout"]) == {"feedback", "command"}
|
|
||||||
assert accepted["urdf_rebuilt_from_json"] is True
|
|
||||||
assert accepted["cad_zero_assumptions"] == dict(profile.zero.cad_zero_assumptions)
|
|
||||||
for name in profile.zero.cad_zero_assumptions:
|
|
||||||
assert fit.zero_offsets_rad[name] == 0
|
|
||||||
assert fit.zero_method_by_joint[name] == "assumed_source_cad_zero"
|
|
||||||
mapper = load_unified_mapper(result.staged_release.calibration_json)
|
|
||||||
assert max(abs(v) for v in mapper.map_positions(profile.command.baseline_values)) < 1e-10
|
|
||||||
rebuilt = rebuild_urdf_from_json(result.staged_release.calibration_json, source, tmp_path/"rebuilt.urdf",
|
|
||||||
authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id)
|
|
||||||
assert rebuilt.read_bytes() == result.path.read_bytes()
|
|
||||||
assert hashlib.sha256(source.read_bytes()).hexdigest() == hashes["source_urdf_sha256"]
|
|
||||||
# Editing JSON and its report together cannot claim different corrections
|
|
||||||
# for an unchanged URDF, even when the command tables still agree.
|
|
||||||
report_path = result.path.parent/"calibration_report.json"
|
|
||||||
report = json.loads(report_path.read_text())
|
|
||||||
edits = payload["urdf_correction"]["joint_patches"]
|
|
||||||
name = next(iter(edits))
|
|
||||||
edits[name]["origin_rpy"] = "0 0 0.123"
|
|
||||||
report["urdf_correction"] = payload["urdf_correction"]
|
|
||||||
result.staged_release.calibration_json.write_text(json.dumps(payload))
|
|
||||||
report_path.write_text(json.dumps(report))
|
|
||||||
with pytest.raises(ValueError, match="calibration_json_correction_differs_from_urdf"):
|
|
||||||
validators[0](result.staged_release.calibration_json, result.path)
|
|
||||||
@@ -1,103 +0,0 @@
|
|||||||
"""Final image evidence includes other motions, with no invented 3-D poses."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.runtime.artifacts.evidence import _replay_directions
|
|
||||||
from linkerhand_calibration.runtime.artifacts.image_evidence import ImageEvidence
|
|
||||||
from linkerhand_calibration.runtime.image_capture import IMAGE_OBSERVATION_POLICY
|
|
||||||
from test_final_image_sources import scene
|
|
||||||
|
|
||||||
|
|
||||||
def source_scene(input_kind="feedback"):
|
|
||||||
config, records, observations = scene()
|
|
||||||
profile = config.calibration_contract.typed_profile
|
|
||||||
tasks = {task.key: task for task in profile.motion.tasks}
|
|
||||||
mappings = {joint: SimpleNamespace(motor_index=task.command_index)
|
|
||||||
for task in tasks.values() for joint in task.joints}
|
|
||||||
cycle = 3 if input_kind == "feedback" else 5
|
|
||||||
phase = "sweep" if input_kind == "feedback" else "steady"
|
|
||||||
frames = {f"{r['view']}:{r['image_stamp_ns']}": r for r in records if r['kind'] == 'pnp_candidate_frame'}
|
|
||||||
raw, primary = [], []
|
|
||||||
for observation in observations:
|
|
||||||
frame = frames[observation.sample_id]
|
|
||||||
frame['cycle'] = cycle
|
|
||||||
history = ['increasing'] * profile.command.command_count
|
|
||||||
history[tasks[frame['task_name']].command_index] = frame['direction']
|
|
||||||
values = list(observation.sdk_values)
|
|
||||||
values[0] = 120.25 # Exposure-time feedback is not an integer command.
|
|
||||||
image = dict(kind='image_observation_frame', policy=IMAGE_OBSERVATION_POLICY,
|
|
||||||
view=frame['view'], image_stamp_ns=frame['image_stamp_ns'], task_name=frame['task_name'],
|
|
||||||
cycle=cycle, direction=frame['direction'], attempt=1, sample_phase=phase,
|
|
||||||
tags=deepcopy(frame['roles']), command_unit=profile.command.unit,
|
|
||||||
command_vector=[120] * profile.command.command_count, feedback_vector=values,
|
|
||||||
command_direction_by_index=history, state_image_sync_error_ns=10_000_000,
|
|
||||||
camera_matrix=frame['camera_matrix'], camera_matrix_source='CameraInfo.P[:3,:3]',
|
|
||||||
input_is_rectified=True)
|
|
||||||
sdk = image['feedback_vector' if input_kind == 'feedback' else 'command_vector']
|
|
||||||
directions = _replay_directions(mappings, image, tasks[image['task_name']])
|
|
||||||
primary.append(replace(observation, cycle=cycle, sdk_values=tuple(sdk), sdk_directions_by_joint=directions))
|
|
||||||
raw.append(image)
|
|
||||||
records.append(dict(kind='scan_unit_complete', task_name=frame['task_name'], cycle=cycle,
|
|
||||||
direction=frame['direction'], attempt=1, passed=True))
|
|
||||||
# ID7 is on the distal link. Its pixels during the flexion task must be
|
|
||||||
# checked even though that task's primary camera is front.
|
|
||||||
extra = deepcopy(next(row for row in raw if row['view'] == 'top'))
|
|
||||||
extra.update(image_stamp_ns=extra['image_stamp_ns']+1, task_name='thumb_pitch_ip_front')
|
|
||||||
for point in extra['tags']['thumb_yaw']['corners_xy']:
|
|
||||||
point[0] += 8
|
|
||||||
records.extend(raw + [extra])
|
|
||||||
records.append(dict(kind='scan_unit_complete', task_name=extra['task_name'], cycle=cycle,
|
|
||||||
direction=extra['direction'], attempt=1, passed=True))
|
|
||||||
return config, records, primary, mappings, extra
|
|
||||||
|
|
||||||
|
|
||||||
def prepare(config, records, primary, mappings, input_kind):
|
|
||||||
evidence = ImageEvidence(config.calibration_contract.typed_profile, records,
|
|
||||||
camera_extrinsics_file=config.camera_extrinsics,
|
|
||||||
protected_inputs={'camera_extrinsics_sha256': config.camera_extrinsics_sha256})
|
|
||||||
return evidence.all_view_holdout(primary, mappings=mappings, input_kind=input_kind,
|
|
||||||
holdout_cycle=3 if input_kind == 'feedback' else 5)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('input_kind', ['feedback', 'command'])
|
|
||||||
def test_other_motion_pixels_reach_the_final_check_without_pnp(input_kind):
|
|
||||||
config, records, primary, mappings, extra = source_scene(input_kind)
|
|
||||||
images = prepare(config, records, primary, mappings, input_kind)
|
|
||||||
assert len(images) == 3
|
|
||||||
added = next(row for row in images if row.sample_id == f"top:{extra['image_stamp_ns']}")
|
|
||||||
assert added.role == 'thumb_yaw'
|
|
||||||
assert added.task_name == 'thumb_pitch_ip_front'
|
|
||||||
assert added.corners_xy == tuple(map(tuple, extra['tags']['thumb_yaw']['corners_xy']))
|
|
||||||
assert added.sdk_values[0] == (120.25 if input_kind == 'feedback' else 120)
|
|
||||||
assert dict(added.directions)['rh_thumb_cmc_yaw'] == extra['command_direction_by_index'][1]
|
|
||||||
assert not any(r['kind'] == 'pnp_candidate_frame' and r['image_stamp_ns'] == extra['image_stamp_ns'] for r in records)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('mutation', ['pixels', 'sdk', 'policy', 'unit', 'direction', 'missing', 'duplicate', 'skew', 'nonfinite'])
|
|
||||||
def test_conflicting_or_missing_primary_raw_evidence_is_rejected(mutation):
|
|
||||||
config, records, primary, mappings, extra = source_scene()
|
|
||||||
row = next(r for r in records if r['kind'] == 'image_observation_frame' and r['view'] == 'top' and r is not extra)
|
|
||||||
if mutation == 'pixels': row['tags']['thumb_yaw']['corners_xy'][0][0] += 5
|
|
||||||
elif mutation == 'sdk': row['feedback_vector'][0] += .5
|
|
||||||
elif mutation == 'policy': row['policy'] = 'unknown'
|
|
||||||
elif mutation == 'unit': row['command_unit'] = 'rad'
|
|
||||||
elif mutation == 'direction': row['command_direction_by_index'] = ['increasing']
|
|
||||||
elif mutation == 'missing': records.remove(row)
|
|
||||||
elif mutation == 'skew': row['state_image_sync_error_ns'] = -1
|
|
||||||
elif mutation == 'nonfinite': row['feedback_vector'][0] = float('nan')
|
|
||||||
else: records.append({**row, 'feedback_vector': [1] * 6})
|
|
||||||
with pytest.raises(ValueError, match='final_image'):
|
|
||||||
prepare(config, records, primary, mappings, 'feedback')
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('mutation', ['training', 'failed_attempt', 'missing_feedback'])
|
|
||||||
def test_unavailable_or_non_holdout_auxiliary_images_do_not_become_measurements(mutation):
|
|
||||||
config, records, primary, mappings, extra = source_scene()
|
|
||||||
if mutation == 'training': extra['cycle'] = 2
|
|
||||||
elif mutation == 'failed_attempt': extra['attempt'] = 2
|
|
||||||
else: extra['feedback_vector'] = None
|
|
||||||
assert len(prepare(config, records, primary, mappings, 'feedback')) == 2
|
|
||||||
@@ -1,74 +0,0 @@
|
|||||||
"""Other cameras retain raw pixels without posing as primary joint samples."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection
|
|
||||||
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"])
|
|
||||||
@pytest.mark.parametrize("phase", ["sweep", "zero_approach"])
|
|
||||||
def test_non_task_camera_records_measured_corners_without_a_pose(tmp_path, monkeypatch, model, phase):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model=model)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
host.execution.session.phase = Phase.SWEEP
|
|
||||||
task = host.profile.motion.tasks[0]
|
|
||||||
direction = "decreasing" if phase == "sweep" else None
|
|
||||||
host._motion = MotionCommand(phase, host.profile.command.baseline_values, 1.,
|
|
||||||
task_key=task.key, command_index=task.command_index, cycle=2, direction=direction,
|
|
||||||
reference_joints=task.joints if phase == "zero_approach" else ())
|
|
||||||
view = next(view for view in host.profile.vision.views if view.name != task.view)
|
|
||||||
tag = next(tag for tag in view.tags if not tag.fixed_reference)
|
|
||||||
points = ((200., 200.), (240., 200.), (240., 240.), (200., 240.))
|
|
||||||
# Camera-derived geometry may be unresolved in this other motion.
|
|
||||||
# Raw measured pixels must survive without inventing a Tag pose.
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda *a, **k: ((), None))
|
|
||||||
host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), (
|
|
||||||
TagDetection(tag.tag_id, points, 0, 100.),)))
|
|
||||||
host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), (
|
|
||||||
TagDetection(tag.tag_id, points, 0, 100.),)))
|
|
||||||
rows = [r for r in host.raw_records if r["kind"] == "image_observation_frame"]
|
|
||||||
assert len(rows) == 1
|
|
||||||
row = rows[0]
|
|
||||||
assert row["task_name"] == task.key and row["view"] == view.name
|
|
||||||
assert (row["cycle"], row["direction"], row["sample_phase"]) == (2, direction, phase)
|
|
||||||
np.testing.assert_array_equal(row["tags"][tag.role]["corners_xy"], points)
|
|
||||||
assert "pose" not in row["tags"][tag.role]
|
|
||||||
assert not any(r["kind"] == "joint_sample" for r in host.raw_records)
|
|
||||||
assert host.execution.session.phase == Phase.SWEEP
|
|
||||||
assert not clock.positions # Observing an additional view schedules no motion.
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_raw_images_keep_detection_quality_and_stable_window_boundaries(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
host.execution.session.phase = Phase.SWEEP
|
|
||||||
task = host.profile.motion.tasks[0]
|
|
||||||
host._motion = MotionCommand("steady", host.profile.command.baseline_values, 1.,
|
|
||||||
task_key=task.key, command_index=task.command_index, cycle=5, direction="increasing")
|
|
||||||
view = next(view for view in host.profile.vision.views if view.name == task.view)
|
|
||||||
tag = next(tag for tag in view.tags if not tag.fixed_reference)
|
|
||||||
points = ((200., 200.), (240., 200.), (240., 240.), (200., 240.))
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda *a, **k: ((), None))
|
|
||||||
observation = DetectionInput(view.name, host.ports.clock_ns(), (TagDetection(tag.tag_id, points, 0, 100.),))
|
|
||||||
host.receive_detections(observation)
|
|
||||||
assert not any(r["kind"] == "image_observation_frame" for r in host.raw_records)
|
|
||||||
host._steady_capture_after_ns = host.ports.clock_ns()
|
|
||||||
rejected = replace(observation.detections[0], hamming=100)
|
|
||||||
host.receive_detections(replace(observation, detections=(rejected,)))
|
|
||||||
row = next(r for r in host.raw_records if r["kind"] == "image_observation_frame")
|
|
||||||
assert not row["tags"]
|
|
||||||
assert row["cycle"] == 5 and row["session_epoch"] == host._observation_epoch
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,307 +0,0 @@
|
|||||||
from pathlib import Path
|
|
||||||
from types import SimpleNamespace
|
|
||||||
import ast
|
|
||||||
|
|
||||||
from linkerhand_calibration.compat import product_profile_key
|
|
||||||
from linkerhand_calibration.core import (
|
|
||||||
ArtifactPolicy,
|
|
||||||
CalibrationProfile,
|
|
||||||
CommandLayout,
|
|
||||||
MeasurementPolicy,
|
|
||||||
MeasurementSpec,
|
|
||||||
MotionPolicy,
|
|
||||||
ProfileKey,
|
|
||||||
QualityPolicy,
|
|
||||||
ScopePolicy,
|
|
||||||
TagSpec,
|
|
||||||
TaskSpec,
|
|
||||||
ViewSpec,
|
|
||||||
VisionRigSpec,
|
|
||||||
ZeroSolvePolicy,
|
|
||||||
validate_profile,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.core.urdf import UrdfCorrectionPlan
|
|
||||||
from linkerhand_calibration.extrinsics import (
|
|
||||||
validate_camera_extrinsics_payload,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.compat.legacy_diagnostic_tools.models import (
|
|
||||||
EngineBindings,
|
|
||||||
ProfileRegistry,
|
|
||||||
RegisteredProfile,
|
|
||||||
get_default_registry,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.product import get_product_calibration_contract
|
|
||||||
from linkerhand_calibration.runtime import CalibrationPhase, CalibrationSession
|
|
||||||
from linkerhand_calibration.runtime.ros import entrypoint as calibration_node
|
|
||||||
|
|
||||||
|
|
||||||
PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration"
|
|
||||||
|
|
||||||
|
|
||||||
def _source_text(directory: Path) -> str:
|
|
||||||
return "\n".join(
|
|
||||||
path.read_text(encoding="utf-8")
|
|
||||||
for path in sorted(directory.rglob("*.py"))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _small_profile() -> CalibrationProfile:
|
|
||||||
active = frozenset({"axis_a", "axis_b"})
|
|
||||||
return CalibrationProfile(
|
|
||||||
key=ProfileKey("TEST", "right", "two_view_three_command", 1),
|
|
||||||
namespace="/calibration_test",
|
|
||||||
command=CommandLayout(
|
|
||||||
names=("axis_a", "unused", "axis_b"),
|
|
||||||
baseline_u8=(255, 0, 127),
|
|
||||||
command_index_by_joint={"axis_a": 0, "axis_b": 2},
|
|
||||||
disabled_indices=frozenset({1}),
|
|
||||||
),
|
|
||||||
vision=VisionRigSpec(
|
|
||||||
views=(
|
|
||||||
ViewSpec(
|
|
||||||
"oblique",
|
|
||||||
(TagSpec("base_a", 40, fixed_reference=True), TagSpec("moving_a", 42)),
|
|
||||||
),
|
|
||||||
ViewSpec("wrist", (TagSpec("base_b", 43, fixed_reference=True), TagSpec("moving_b", 41))),
|
|
||||||
),
|
|
||||||
common_frame="fixture",
|
|
||||||
extrinsic_reference_view="oblique",
|
|
||||||
),
|
|
||||||
motion=MotionPolicy(
|
|
||||||
tasks=(
|
|
||||||
TaskSpec("scan_a", "oblique", 0, ("axis_a",)),
|
|
||||||
TaskSpec("scan_b", "wrist", 2, ("axis_b",)),
|
|
||||||
)
|
|
||||||
),
|
|
||||||
measurement=MeasurementPolicy(
|
|
||||||
measurements={
|
|
||||||
"axis_a": MeasurementSpec(
|
|
||||||
"axis_a", "rotation", "oblique", "base_a", "moving_a"
|
|
||||||
),
|
|
||||||
"axis_b": MeasurementSpec(
|
|
||||||
"axis_b", "circle", "wrist", "base_b", "moving_b"
|
|
||||||
),
|
|
||||||
}
|
|
||||||
),
|
|
||||||
zero=ZeroSolvePolicy(
|
|
||||||
active_joints=active,
|
|
||||||
passive_joints=frozenset(),
|
|
||||||
direct_zero_joints=("axis_a", "axis_b"),
|
|
||||||
axis_joints=("axis_a", "axis_b"),
|
|
||||||
mechanical_endpoint_joints=frozenset({"axis_b"}),
|
|
||||||
post_solve_endpoint_joints=frozenset(),
|
|
||||||
mimic_source_by_joint={},
|
|
||||||
cad_frozen_joints=frozenset(),
|
|
||||||
),
|
|
||||||
quality=QualityPolicy(
|
|
||||||
training_cycles=(0, 1, 2),
|
|
||||||
holdout_cycle=3,
|
|
||||||
hard_threshold_keys=frozenset({"maximum_error"}),
|
|
||||||
),
|
|
||||||
scope=ScopePolicy(
|
|
||||||
calibrate_joints={"full": active, "axis_a": frozenset({"axis_a"})},
|
|
||||||
frozen_joints={
|
|
||||||
"full": frozenset(),
|
|
||||||
"axis_a": frozenset({"axis_b"}),
|
|
||||||
},
|
|
||||||
),
|
|
||||||
artifacts=ArtifactPolicy(
|
|
||||||
output_schema_version=2,
|
|
||||||
calibration_filename="calibration.json",
|
|
||||||
corrected_urdf_filename="corrected.urdf",
|
|
||||||
protected_input_fields=frozenset({"source_urdf_sha256"}),
|
|
||||||
),
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _bindings() -> EngineBindings:
|
|
||||||
no_waypoints = lambda *args, **kwargs: ()
|
|
||||||
return EngineBindings(
|
|
||||||
hand_profile=SimpleNamespace(),
|
|
||||||
zero_profile=SimpleNamespace(),
|
|
||||||
motion_command=lambda *args, **kwargs: [0, 0, 0],
|
|
||||||
preparation_waypoints=no_waypoints,
|
|
||||||
return_waypoints=no_waypoints,
|
|
||||||
cli_main=lambda args=None: None,
|
|
||||||
node_main=lambda args=None: None,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_core_has_no_ros_or_model_dependency() -> None:
|
|
||||||
# Historical provenance in a comment is not a dependency. Check imports
|
|
||||||
# and executable product-identity branches instead of matching prose.
|
|
||||||
for path in (PACKAGE / "core").rglob("*.py"):
|
|
||||||
tree = ast.parse(path.read_text())
|
|
||||||
for node in ast.walk(tree):
|
|
||||||
if isinstance(node, ast.ImportFrom):
|
|
||||||
assert not {"models", "rclpy", "cv_bridge"} & set((node.module or "").split(".")), path
|
|
||||||
if isinstance(node, ast.Import):
|
|
||||||
assert not any({"models", "rclpy", "cv_bridge"} & set(alias.name.split(".")) for alias in node.names), path
|
|
||||||
if isinstance(node, (ast.If, ast.IfExp)):
|
|
||||||
assert not any(isinstance(item, ast.Constant) and isinstance(item.value, str)
|
|
||||||
and item.value.upper() in {"G20", "L6", "O6", "O12"}
|
|
||||||
for item in ast.walk(node.test)), path
|
|
||||||
|
|
||||||
|
|
||||||
def test_runtime_has_no_concrete_model_or_view_assumption() -> None:
|
|
||||||
# A JSON "side" field describes handedness, not a hardcoded camera.
|
|
||||||
# Inspect executable selection rather than rejecting schema keys/comments.
|
|
||||||
forbidden = {"G20", "L6", "O6", "O12", "RIGHT_19", "front", "side", "top"}
|
|
||||||
def selection_literals(node):
|
|
||||||
if isinstance(node, ast.Constant):
|
|
||||||
yield node.value
|
|
||||||
elif isinstance(node, (ast.Tuple, ast.List, ast.Set)):
|
|
||||||
for item in node.elts:
|
|
||||||
yield from selection_literals(item)
|
|
||||||
for path in (PACKAGE / "runtime").rglob("*.py"):
|
|
||||||
tree = ast.parse(path.read_text())
|
|
||||||
for node in ast.walk(tree):
|
|
||||||
if isinstance(node, (ast.If, ast.IfExp)):
|
|
||||||
# A payload.get("side") field lookup is not selection by
|
|
||||||
# camera identity. Inspect compared literal values instead.
|
|
||||||
assert not any(isinstance(value, str) and value in forbidden
|
|
||||||
for comparison in ast.walk(node.test) if isinstance(comparison, ast.Compare)
|
|
||||||
for operand in comparison.comparators for value in selection_literals(operand)), path
|
|
||||||
if isinstance(node, ast.Subscript) and isinstance(node.slice, ast.Constant):
|
|
||||||
if node.slice.value in {"front", "side", "top"}:
|
|
||||||
assert not any(isinstance(item, ast.Attribute) and item.attr in {
|
|
||||||
"views", "cameras", "camera_matrices", "trackers"}
|
|
||||||
for item in ast.walk(node.value)), path
|
|
||||||
|
|
||||||
|
|
||||||
def test_every_registered_profile_passes_static_integrity_checks() -> None:
|
|
||||||
registry = get_default_registry()
|
|
||||||
assert len(registry) == 8
|
|
||||||
for registered in registry:
|
|
||||||
validate_profile(registered.profile)
|
|
||||||
assert registered.profile.zero.active_joints
|
|
||||||
assert registered.profile.motion.tasks
|
|
||||||
assert registered.profile.vision.tag_ids
|
|
||||||
|
|
||||||
|
|
||||||
def test_registry_accepts_different_view_and_command_counts() -> None:
|
|
||||||
profile = _small_profile()
|
|
||||||
registry = ProfileRegistry()
|
|
||||||
registry.register(RegisteredProfile(profile, _bindings()))
|
|
||||||
|
|
||||||
selected = registry.get(profile.key).profile
|
|
||||||
assert selected.command.command_count == 3
|
|
||||||
assert selected.vision.view_names == ("oblique", "wrist")
|
|
||||||
|
|
||||||
|
|
||||||
def test_generic_extrinsics_validator_uses_profile_view_names() -> None:
|
|
||||||
identity = {
|
|
||||||
"translation_xyz_m": [0.0, 0.0, 0.0],
|
|
||||||
"quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
|
||||||
}
|
|
||||||
payload = {
|
|
||||||
"schema_version": 1,
|
|
||||||
"reference_view": "oblique",
|
|
||||||
"cameras": {
|
|
||||||
"oblique": {
|
|
||||||
"serial_number": "A",
|
|
||||||
"width": 640,
|
|
||||||
"height": 480,
|
|
||||||
"intrinsics_sha256": "a" * 64,
|
|
||||||
},
|
|
||||||
"wrist": {
|
|
||||||
"serial_number": "B",
|
|
||||||
"width": 800,
|
|
||||||
"height": 600,
|
|
||||||
"intrinsics_sha256": "b" * 64,
|
|
||||||
},
|
|
||||||
},
|
|
||||||
"oblique_from_view": {
|
|
||||||
"oblique": identity,
|
|
||||||
"wrist": {
|
|
||||||
"translation_xyz_m": [0.1, 0.0, 0.0],
|
|
||||||
"quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
|
||||||
},
|
|
||||||
},
|
|
||||||
"quality": {"passed": True, "rms": 0.2, "pair_captures": 4},
|
|
||||||
}
|
|
||||||
|
|
||||||
validate_camera_extrinsics_payload(
|
|
||||||
payload,
|
|
||||||
required_views=("oblique", "wrist"),
|
|
||||||
reference_view="oblique",
|
|
||||||
quality_limits={"rms": 0.5},
|
|
||||||
minimum_capture_counts={"pair_captures": 3},
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_legacy_node_binding_handoff_has_no_model_identity_branch(
|
|
||||||
monkeypatch, tmp_path,
|
|
||||||
) -> None:
|
|
||||||
import hashlib
|
|
||||||
profile = _small_profile()
|
|
||||||
from linkerhand_calibration import profiles
|
|
||||||
path = tmp_path/"profile.yaml"
|
|
||||||
path.write_text(profiles.dump_hand_profile(profile))
|
|
||||||
forwarded: list[list[str] | None] = []
|
|
||||||
bindings = _bindings()
|
|
||||||
bindings = EngineBindings(
|
|
||||||
hand_profile=bindings.hand_profile,
|
|
||||||
zero_profile=bindings.zero_profile,
|
|
||||||
motion_command=bindings.motion_command,
|
|
||||||
preparation_waypoints=bindings.preparation_waypoints,
|
|
||||||
return_waypoints=bindings.return_waypoints,
|
|
||||||
cli_main=bindings.cli_main,
|
|
||||||
node_main=forwarded.append,
|
|
||||||
)
|
|
||||||
registry = ProfileRegistry()
|
|
||||||
registry.register(RegisteredProfile(profile, bindings))
|
|
||||||
monkeypatch.setattr(calibration_node, "run_profile_node", lambda declared, args: forwarded.append(args))
|
|
||||||
|
|
||||||
calibration_node.main(
|
|
||||||
[
|
|
||||||
"--profile-id",
|
|
||||||
profile.key.profile_id,
|
|
||||||
"--profile-config", str(path),
|
|
||||||
"--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(),
|
|
||||||
"--ros-args",
|
|
||||||
"-p",
|
|
||||||
"sample_rate:=30",
|
|
||||||
]
|
|
||||||
)
|
|
||||||
|
|
||||||
assert forwarded == [["--ros-args", "-p", "sample_rate:=30"]]
|
|
||||||
|
|
||||||
|
|
||||||
def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None:
|
|
||||||
key = product_profile_key(
|
|
||||||
{
|
|
||||||
"schema_version": 2,
|
|
||||||
"profile_id": "TEST/right/two_view_three_command/v3",
|
|
||||||
}
|
|
||||||
)
|
|
||||||
assert key == ProfileKey("TEST", "right", "two_view_three_command", 3)
|
|
||||||
|
|
||||||
|
|
||||||
def test_retired_layout_alias_is_confined_to_compatibility_resolution() -> None:
|
|
||||||
contract = get_product_calibration_contract(
|
|
||||||
"G20", "right", "g20_right_15"
|
|
||||||
)
|
|
||||||
assert contract.typed_profile.key.layout == "g20_right_19"
|
|
||||||
|
|
||||||
|
|
||||||
def test_public_runtime_exports_the_shared_session_kernel() -> None:
|
|
||||||
session = CalibrationSession(_small_profile())
|
|
||||||
assert session.phase == CalibrationPhase.WAIT_DEVICE
|
|
||||||
session.device_ready()
|
|
||||||
session.start()
|
|
||||||
session.baseline_complete()
|
|
||||||
session.reference_locked()
|
|
||||||
session.preparation_complete()
|
|
||||||
assert session.phase == CalibrationPhase.SWEEP
|
|
||||||
|
|
||||||
|
|
||||||
def test_urdf_writer_and_validator_can_share_one_edit_plan() -> None:
|
|
||||||
plan = UrdfCorrectionPlan(
|
|
||||||
source_sha256="a" * 64,
|
|
||||||
allowed_active_joints=frozenset({"axis_a", "axis_b"}),
|
|
||||||
endpoint_limit_joints=frozenset({"axis_b"}),
|
|
||||||
mimic_source_by_joint={"passive_b": "axis_b"},
|
|
||||||
frozen_joints=frozenset({"axis_c"}),
|
|
||||||
)
|
|
||||||
plan.authorize_offsets({"axis_a": 0.0, "axis_b": 0.1})
|
|
||||||
@@ -1,68 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.artifacts.storage import atomic_write_json
|
|
||||||
from linkerhand_calibration.runtime.artifacts import ArtifactPublisher
|
|
||||||
|
|
||||||
|
|
||||||
def test_pointer_changes_only_after_both_artifacts_validate(tmp_path: Path) -> None:
|
|
||||||
session = tmp_path / "serial" / "session"
|
|
||||||
session.mkdir(parents=True)
|
|
||||||
calibration = session / "calibration.json"
|
|
||||||
urdf = session / "corrected.urdf"
|
|
||||||
atomic_write_json(calibration, {"schema_version": 1, "quality": {"passed": True}})
|
|
||||||
urdf.write_text('<robot name="test"><link name="base"/></robot>', encoding="utf-8")
|
|
||||||
publisher = ArtifactPublisher(session.parent, "latest_passed")
|
|
||||||
release = publisher.publish(
|
|
||||||
session_directory=session,
|
|
||||||
calibration_json=calibration,
|
|
||||||
corrected_urdf=urdf,
|
|
||||||
validate=lambda _json, _urdf: None,
|
|
||||||
)
|
|
||||||
assert release.pointer.resolve() == session.resolve()
|
|
||||||
assert len(release.sha256["corrected_urdf_sha256"]) == 64
|
|
||||||
|
|
||||||
|
|
||||||
def test_failed_pair_validation_never_updates_pointer(tmp_path: Path) -> None:
|
|
||||||
session = tmp_path / "serial" / "session"
|
|
||||||
session.mkdir(parents=True)
|
|
||||||
calibration = session / "calibration.json"
|
|
||||||
urdf = session / "corrected.urdf"
|
|
||||||
atomic_write_json(calibration, {"schema_version": 1})
|
|
||||||
urdf.write_text('<robot name="test"><link name="base"/></robot>', encoding="utf-8")
|
|
||||||
publisher = ArtifactPublisher(session.parent, "latest_passed")
|
|
||||||
|
|
||||||
def reject(_json: Path, _urdf: Path) -> None:
|
|
||||||
raise ValueError("holdout failed")
|
|
||||||
|
|
||||||
with pytest.raises(ValueError, match="holdout"):
|
|
||||||
publisher.publish(
|
|
||||||
session_directory=session,
|
|
||||||
calibration_json=calibration,
|
|
||||||
corrected_urdf=urdf,
|
|
||||||
validate=reject,
|
|
||||||
)
|
|
||||||
assert not (session.parent / "latest_passed").exists()
|
|
||||||
|
|
||||||
|
|
||||||
def test_staging_does_not_publish_and_changed_or_cancelled_stage_is_rejected(tmp_path):
|
|
||||||
session = tmp_path / "serial" / "session"
|
|
||||||
session.mkdir(parents=True)
|
|
||||||
calibration, urdf = session / "calibration.json", session / "corrected.urdf"
|
|
||||||
atomic_write_json(calibration, {"schema_version": 1})
|
|
||||||
urdf.write_text('<robot name="test"><link name="base"/></robot>')
|
|
||||||
publisher = ArtifactPublisher(session.parent, "latest_passed")
|
|
||||||
staged = publisher.prepare(session_directory=session, calibration_json=calibration,
|
|
||||||
corrected_urdf=urdf, validate=lambda *_: {"passed": True})
|
|
||||||
pointer = session.parent / "latest_passed"
|
|
||||||
assert not pointer.exists()
|
|
||||||
with pytest.raises(ValueError, match="abort|cancel"):
|
|
||||||
publisher.commit(staged, cancelled=lambda: True)
|
|
||||||
assert not pointer.exists()
|
|
||||||
atomic_write_json(calibration, {"schema_version": 2})
|
|
||||||
with pytest.raises(ValueError, match="changed|hash|mutat"):
|
|
||||||
publisher.commit(staged)
|
|
||||||
assert not pointer.exists()
|
|
||||||
@@ -1,60 +0,0 @@
|
|||||||
"""Absolute zero must be invariant to reference points on the same axis line."""
|
|
||||||
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.optimize import brentq
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.fitting.session import compile_spatial_profile
|
|
||||||
from linkerhand_calibration.core.fitting.spatial_solver.residuals import ObservationGeometry
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "g20_right_19", "o12_right_16"])
|
|
||||||
@pytest.mark.parametrize("view", ["oblique_end_on", "side"])
|
|
||||||
def test_physical_zero_is_recovered_for_any_axis_reference_point(layout, view):
|
|
||||||
package = Path(__file__).resolve().parents[1]
|
|
||||||
model_name = layout.split("_")[0]
|
|
||||||
from linkerhand_calibration.product import load_product_config
|
|
||||||
source = load_product_config(package / "config" / f"{model_name}_right_product.yaml", check_can=False).source_urdf
|
|
||||||
model = UrdfKinematicModel(source)
|
|
||||||
spatial = compile_spatial_profile(load_bundled_hand_profile(layout))
|
|
||||||
geometry = ObservationGeometry(spatial, model, {}, {}, {})
|
|
||||||
common = Rotation.from_euler("xyz", [.2, -.15, .4])
|
|
||||||
translation = np.array([.4, -.2, .8])
|
|
||||||
truth = .07
|
|
||||||
|
|
||||||
def line(joint, parent, angle):
|
|
||||||
axis, point = model.axis_line(joint, zero_offsets={parent: angle},
|
|
||||||
joint_angles={}, independent_mimic_angles=True)
|
|
||||||
return common.apply(axis), common.apply(point)+translation
|
|
||||||
|
|
||||||
assert spatial.phase_parent_joint
|
|
||||||
for child, parent in spatial.phase_parent_joint.items():
|
|
||||||
parent_axis, parent_point = line(parent, parent, truth)
|
|
||||||
child_axis, child_point = line(child, parent, truth)
|
|
||||||
assert abs(parent_axis @ child_axis) == pytest.approx(1., abs=1e-8)
|
|
||||||
transverse = np.cross(parent_axis, np.eye(3)[np.argmin(abs(parent_axis))])
|
|
||||||
transverse /= np.linalg.norm(transverse)
|
|
||||||
normal = parent_axis + .3*transverse if view == "oblique_end_on" else transverse
|
|
||||||
normal /= np.linalg.norm(normal)
|
|
||||||
recovered = []
|
|
||||||
for parent_shift, child_shift in [(0., 0.), (.03, -.09), (-.07, .04)]:
|
|
||||||
observed_parent = parent_point + parent_shift*parent_axis
|
|
||||||
observed_child = child_point + child_shift*child_axis
|
|
||||||
|
|
||||||
def residual(angle):
|
|
||||||
predicted_axis, predicted_parent = line(parent, parent, angle)
|
|
||||||
_, predicted_child = line(child, parent, angle)
|
|
||||||
# The predicted lines also admit arbitrary reference points.
|
|
||||||
predicted_parent = predicted_parent + .08*predicted_axis
|
|
||||||
predicted_child = predicted_child - .06*predicted_axis
|
|
||||||
return geometry.phase_error(predicted_axis, predicted_parent, predicted_child,
|
|
||||||
parent_axis, observed_parent, observed_child, normal)
|
|
||||||
|
|
||||||
assert residual(truth) == pytest.approx(0., abs=1e-9), (layout, parent, child)
|
|
||||||
recovered.append(brentq(residual, -.25, .25))
|
|
||||||
assert recovered == pytest.approx([truth]*3, abs=1e-9), (layout, parent, child)
|
|
||||||
@@ -1,44 +0,0 @@
|
|||||||
"""Nonlinear measured motion must not make the URDF fallback exceed limits."""
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.fitting.coupling import fit_standard_mimic
|
|
||||||
from linkerhand_calibration.core.fitting.mimic_approximation import shared_zero_multiplier_bounds
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("parent,child,expected", [
|
|
||||||
((0, 2), (0, 1), (0, .5)),
|
|
||||||
((-2, 0), (0, 1), (-.5, 0)),
|
|
||||||
((-2, 3), (-1, 2), (-1/3, .5)),
|
|
||||||
])
|
|
||||||
def test_feasible_multiplier_respects_both_endpoints(parent, child, expected):
|
|
||||||
bounds = shared_zero_multiplier_bounds(parent, child)
|
|
||||||
assert bounds == pytest.approx(expected)
|
|
||||||
for slope in bounds:
|
|
||||||
for endpoint in parent:
|
|
||||||
assert child[0]-1e-12 <= slope*endpoint <= child[1]+1e-12
|
|
||||||
|
|
||||||
|
|
||||||
def test_nonlinear_fit_is_constrained_without_changing_observations():
|
|
||||||
x = np.tile(np.linspace(0, 2, 80), 3)
|
|
||||||
y = x - .25*x*x
|
|
||||||
original = y.copy()
|
|
||||||
cycles = np.repeat([0, 1, 2], 80)
|
|
||||||
args = dict(source_joint="parent", target_joint="child", active_rad=x,
|
|
||||||
passive_rad=y, cycles=cycles, offset_rad=0)
|
|
||||||
unconstrained = fit_standard_mimic(**args)
|
|
||||||
bounded = fit_standard_mimic(**args,
|
|
||||||
multiplier_bounds=shared_zero_multiplier_bounds((0, 2), (0, 1)))
|
|
||||||
assert unconstrained.multiplier*2 > 1
|
|
||||||
assert bounded.multiplier == pytest.approx(.5, abs=1e-8)
|
|
||||||
assert bounded.offset_rad == 0
|
|
||||||
np.testing.assert_array_equal(y, original)
|
|
||||||
np.testing.assert_allclose(bounded.residuals_rad, bounded.multiplier*x-y)
|
|
||||||
with pytest.raises(ValueError, match="holdout is forbidden"):
|
|
||||||
fit_standard_mimic(**{**args, "cycles": np.repeat([0, 1, 3], 80)}, multiplier_bounds=(0, .5))
|
|
||||||
|
|
||||||
|
|
||||||
def test_impossible_shared_zero_range_is_rejected():
|
|
||||||
with pytest.raises(ValueError, match="excludes_shared_zero"):
|
|
||||||
shared_zero_multiplier_bounds((0, 2), (1, 2))
|
|
||||||
@@ -1,225 +0,0 @@
|
|||||||
"""Declared arrival evidence, source identity and coordinator cancellation."""
|
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import asdict, replace
|
|
||||||
from threading import Event
|
|
||||||
import json
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime import coordinator as coordinator_module
|
|
||||||
from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput
|
|
||||||
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
||||||
from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256
|
|
||||||
from test_diagnostic_coordinator import projected_frame, start_diagnostic, TERMINAL_PHASES
|
|
||||||
from test_motion_branch_evidence import motion_frames
|
|
||||||
from measured_capture_fixture import reordered_profile
|
|
||||||
|
|
||||||
|
|
||||||
def arrival(profile):
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
name = task.joints[0]
|
|
||||||
reference = profile.motion.joint_zero_references[name]
|
|
||||||
common = dict(task_key=task.key, command_index=task.command_index)
|
|
||||||
approach = MotionCommand("zero_approach", reference.command, 1.,
|
|
||||||
reference_joints=(name,), **common)
|
|
||||||
zero = MotionCommand("joint_zero", reference.command, 1., zero_joints=(name,), **common)
|
|
||||||
measurement = profile.measurement.measurements[name]
|
|
||||||
rows = []
|
|
||||||
samples = motion_frames()
|
|
||||||
for i, frame in enumerate(samples):
|
|
||||||
command = list(reference.command)
|
|
||||||
index = task.command_index
|
|
||||||
command[index] = reference.approach_commands[-1][index] + i / (len(samples)-1) * (
|
|
||||||
reference.command[index] - reference.approach_commands[-1][index])
|
|
||||||
tags = {}
|
|
||||||
for role, item in zip((measurement.parent_role, measurement.child_role), frame.roles):
|
|
||||||
tags[role] = {"reprojection_valid_candidates": [asdict(pose) for pose in item.candidates],
|
|
||||||
"selected_pose": asdict(item.candidates[-1]),
|
|
||||||
"candidate_diagnostics": {"observation_stamp_ns": frame.stamp_ns,
|
|
||||||
"branch_frozen": item.frozen}}
|
|
||||||
rows.append(dict(kind="motion_branch_observation", task_name=task.key,
|
|
||||||
view=measurement.view, zero_joints=[name], image_stamp_ns=frame.stamp_ns,
|
|
||||||
command_vector=command, feedback_vector=command.copy(), tags=tags,
|
|
||||||
session_epoch=1, motion_version=7))
|
|
||||||
return approach, zero, rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19",
|
|
||||||
"virtual_o6_right_8", "virtual_g20_right_19"])
|
|
||||||
def test_all_models_bind_motion_before_zero_to_the_actual_source_version(layout):
|
|
||||||
profile = load_bundled_hand_profile(layout.removeprefix("virtual_"))
|
|
||||||
if layout.startswith("virtual_"):
|
|
||||||
profile = reordered_profile(profile)
|
|
||||||
approach, zero, rows = arrival(profile)
|
|
||||||
# This legacy fixture contains only 3D candidates, no captured pixels.
|
|
||||||
# It verifies the historical evidence/version binding separately from
|
|
||||||
# the production image path exercised by the coordinator tests below.
|
|
||||||
owner = BranchInitialization(profile, image_geometry=False)
|
|
||||||
owner.begin(approach, session_epoch=1, motion_version=7)
|
|
||||||
for row in rows:
|
|
||||||
owner.observe(row)
|
|
||||||
owner.begin(zero, session_epoch=1, motion_version=8)
|
|
||||||
request = owner.request(rows[0]["view"], zero, session_epoch=1, motion_version=8)
|
|
||||||
assert request.motion_version == 7
|
|
||||||
result = solve_initialization(request)
|
|
||||||
assert result.resolved, result
|
|
||||||
model, report = owner.accept(request, result)
|
|
||||||
assert owner.ready(zero) and model.evidence_ids
|
|
||||||
expected = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in rows}
|
|
||||||
assert report["source_frame_hashes"]
|
|
||||||
assert all(expected[key] == value for key, value in report["source_frame_hashes"].items())
|
|
||||||
assert set(report["source_frame_hashes"]) == {str(s) for s in report["source_image_stamps"]}
|
|
||||||
assert not set(report["source_image_stamps"]) - {row["image_stamp_ns"] for row in rows}
|
|
||||||
|
|
||||||
|
|
||||||
def test_only_last_arrival_segment_and_complete_holding_state_can_authorize_zero():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
approach, zero, rows = arrival(profile)
|
|
||||||
owner = BranchInitialization(profile)
|
|
||||||
owner.begin(approach, session_epoch=1, motion_version=6)
|
|
||||||
for row in rows:
|
|
||||||
owner.observe(dict(row, motion_version=6))
|
|
||||||
owner.begin(approach, session_epoch=1, motion_version=7)
|
|
||||||
# Prior, stale-version images and images without SDK state cannot survive
|
|
||||||
# the final arrival segment or be substituted for its physical evidence.
|
|
||||||
for row in rows:
|
|
||||||
owner.observe(dict(row, motion_version=6))
|
|
||||||
owner.observe(dict(row, feedback_vector=None))
|
|
||||||
wrong_pose = deepcopy(row)
|
|
||||||
wrong_pose["command_vector"][0] -= 10
|
|
||||||
owner.observe(wrong_pose)
|
|
||||||
owner.begin(zero, session_epoch=1, motion_version=8)
|
|
||||||
request = owner.request("top", zero, session_epoch=1, motion_version=8)
|
|
||||||
assert not request.frames and not solve_initialization(request).resolved
|
|
||||||
assert not owner.ready(zero)
|
|
||||||
|
|
||||||
|
|
||||||
def test_movement_of_a_held_joint_invalidates_motion_evidence():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
approach, zero, rows = arrival(profile)
|
|
||||||
owner = BranchInitialization(profile)
|
|
||||||
owner.begin(approach, session_epoch=1, motion_version=7)
|
|
||||||
for i, row in enumerate(rows):
|
|
||||||
if i == len(rows)//2:
|
|
||||||
row["feedback_vector"][0] -= 5
|
|
||||||
owner.observe(row)
|
|
||||||
owner.begin(zero, session_epoch=1, motion_version=8)
|
|
||||||
request = owner.request("top", zero, session_epoch=1, motion_version=8)
|
|
||||||
result = solve_initialization(request)
|
|
||||||
assert result.reason == "motion_held_feedback_changed" and not result.resolved
|
|
||||||
owner.accept(request, result)
|
|
||||||
assert not owner.ready(zero)
|
|
||||||
assert "top:joints=rh_thumb_cmc_yaw" in owner.error()
|
|
||||||
|
|
||||||
|
|
||||||
def before_initialization(host, clock):
|
|
||||||
"""Run real arrival motion but stop before the first zero image callback."""
|
|
||||||
for _ in range(8000):
|
|
||||||
clock.now += .04
|
|
||||||
feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values
|
|
||||||
host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase not in TERMINAL_PHASES, host.reason
|
|
||||||
if host._motion is not None and host._motion.phase == "joint_zero":
|
|
||||||
return projected_frame(host, "top", feedback)
|
|
||||||
task = next((t for t in host.profile.motion.tasks
|
|
||||||
if host._motion is not None and t.key == host._motion.task_key), None)
|
|
||||||
for view in (task.view,) if task is not None else host.required_observation_views:
|
|
||||||
host.receive_detections(projected_frame(host, view, feedback))
|
|
||||||
pytest.fail("arrival never completed")
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("change", ["abort", "motion_version"])
|
|
||||||
def test_geometry_solves_outside_state_lock_and_cannot_commit_after_invalidation(tmp_path, monkeypatch, change):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
original = coordinator_module.solve_initialization
|
|
||||||
try:
|
|
||||||
frame = before_initialization(host, clock)
|
|
||||||
def blocked(request):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(5)
|
|
||||||
return original(request)
|
|
||||||
monkeypatch.setattr(coordinator_module, "solve_initialization", blocked)
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(host.receive_detections, frame)
|
|
||||||
assert entered.wait(2)
|
|
||||||
# These calls need the real state lock while geometry is blocked.
|
|
||||||
clock.now += .001
|
|
||||||
feedback = tuple(host.last_command)
|
|
||||||
host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns())
|
|
||||||
assert host.state_history[-1].stamp_ns == host.ports.clock_ns()
|
|
||||||
host.snapshot()
|
|
||||||
# Solver wall time must not consume the separate two-second
|
|
||||||
# zero-image window. Feedback and the control timer keep running
|
|
||||||
# while the immutable preparation geometry is being solved.
|
|
||||||
for _ in range(6):
|
|
||||||
clock.now += .5
|
|
||||||
host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase not in TERMINAL_PHASES, host.reason
|
|
||||||
assert host.reason.startswith("joint_zero_geometry_solving:")
|
|
||||||
assert not host.execution.zero_references
|
|
||||||
if change == "abort":
|
|
||||||
assert host.abort().success
|
|
||||||
else:
|
|
||||||
with host.step_data_lock:
|
|
||||||
host._segment_number += 1
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
release.set()
|
|
||||||
future.result(timeout=5)
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
events = [json.loads(line) for line in
|
|
||||||
(host.parameters.session_dir / "motion_solver_diagnostic.jsonl").read_text().splitlines()]
|
|
||||||
assert [event["event"] for event in events] == ["request_created", "solve_finished", "commit_rejected"]
|
|
||||||
assert len({event["source_hashes_sha256"] for event in events}) == 1
|
|
||||||
assert events[1]["elapsed_seconds"] >= 3.0
|
|
||||||
assert not host.branch_initialization.reports
|
|
||||||
assert not host.execution.zero_references
|
|
||||||
assert not host.finalization.started
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_late_previous_segment_image_does_not_consume_current_initialization(tmp_path, monkeypatch):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
try:
|
|
||||||
frame = before_initialization(host, clock)
|
|
||||||
monkeypatch.setattr(coordinator_module, "solve_initialization",
|
|
||||||
lambda request: pytest.fail("stale frame started initialization"))
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
host.receive_detections(DetectionInput("top", host._motion_started_ns-1, frame.detections))
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
assert not host.branch_initialization._requested
|
|
||||||
# Existing image confirmation alone cannot bypass task geometry.
|
|
||||||
assert not host._lock_joint_zeros(host._motion)
|
|
||||||
assert host._zero_reference_error == "motion_geometry_confirmation_pending"
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_motion_geometry_confirmation_cannot_open_the_feedback_stability_gate(tmp_path, monkeypatch):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
try:
|
|
||||||
frame = before_initialization(host, clock)
|
|
||||||
assert host._steady_capture_after_ns is None
|
|
||||||
host.receive_detections(frame)
|
|
||||||
assert host.branch_initialization.ready(host._motion)
|
|
||||||
assert host._steady_capture_after_ns is None
|
|
||||||
assert not host._steady_rows
|
|
||||||
# Camera callbacks between control ticks cannot create a stable SDK
|
|
||||||
# window or contribute a zero, even with a verified geometric model.
|
|
||||||
for _ in range(15):
|
|
||||||
clock.now += .001
|
|
||||||
host.receive_feedback(host.profile.command.names, host.last_command, host.ports.clock_ns())
|
|
||||||
host.receive_detections(projected_frame(host, "top", host.last_command))
|
|
||||||
assert not host._steady_rows
|
|
||||||
assert not host.execution.zero_references
|
|
||||||
assert not any(row["kind"] == "joint_zero_sample" for row in host.raw_records)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
"""Process transport preserves the pure solver and bounds failed workers."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
import multiprocessing
|
|
||||||
import json
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization
|
|
||||||
from linkerhand_calibration.runtime.branch_worker import IsolatedBranchSolver
|
|
||||||
from test_branch_initialization import arrival
|
|
||||||
|
|
||||||
|
|
||||||
def request():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
approach, zero, rows = arrival(profile)
|
|
||||||
owner = BranchInitialization(profile, image_geometry=False)
|
|
||||||
owner.begin(approach, session_epoch=1, motion_version=7)
|
|
||||||
for row in rows:
|
|
||||||
owner.observe(row)
|
|
||||||
owner.begin(zero, session_epoch=1, motion_version=8)
|
|
||||||
return owner.request("top", zero, session_epoch=1, motion_version=8)
|
|
||||||
|
|
||||||
|
|
||||||
def test_process_returns_the_same_resolution_and_source_diagnostics(tmp_path):
|
|
||||||
item = request()
|
|
||||||
expected = solve_initialization(item)
|
|
||||||
path = tmp_path / "solver.jsonl"
|
|
||||||
worker = IsolatedBranchSolver(path)
|
|
||||||
try:
|
|
||||||
actual = worker(item)
|
|
||||||
assert actual == expected and actual.resolved
|
|
||||||
event = json.loads(path.read_text())
|
|
||||||
assert event["event"] == "worker_solve_finished"
|
|
||||||
assert event["source_frames"] == len(item.frames)
|
|
||||||
assert event["thread_cpu_seconds"] > 0
|
|
||||||
invalid = replace(item, invalid_reason="missing_motion_images")
|
|
||||||
assert worker(invalid).reason == "missing_motion_images"
|
|
||||||
finally:
|
|
||||||
worker.close()
|
|
||||||
assert not multiprocessing.active_children()
|
|
||||||
|
|
||||||
|
|
||||||
def test_timeout_and_closed_worker_never_leave_a_child_running():
|
|
||||||
item = request()
|
|
||||||
worker = IsolatedBranchSolver(timeout_seconds=0.)
|
|
||||||
assert worker(item).reason == "image_motion_solver_timeout"
|
|
||||||
assert not multiprocessing.active_children()
|
|
||||||
worker.close()
|
|
||||||
assert worker(item).reason == "image_motion_solver_cancelled"
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_return_request_uses_the_same_isolated_transport(tmp_path):
|
|
||||||
from linkerhand_calibration.runtime.chain_reference import ChainReferenceRequest, ChainReferenceResult
|
|
||||||
item = ChainReferenceRequest('pinky_mcp_pitch_side', 'side', ('pinky_mcp_pitch',),
|
|
||||||
1, 7, 1, (), (), (), (), invalid_reason='training_reference_arc_missing', image_geometry=True)
|
|
||||||
worker = IsolatedBranchSolver(tmp_path/'training_reference.jsonl')
|
|
||||||
try:
|
|
||||||
actual = worker(item)
|
|
||||||
assert isinstance(actual, ChainReferenceResult)
|
|
||||||
assert not actual.resolved and actual.reason == item.invalid_reason
|
|
||||||
finally:
|
|
||||||
worker.close()
|
|
||||||
assert not multiprocessing.active_children()
|
|
||||||
@@ -1,163 +0,0 @@
|
|||||||
"""Source-invariant constraints, recorded ambiguity and independent image truth."""
|
|
||||||
|
|
||||||
from dataclasses import asdict, replace
|
|
||||||
import gzip
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.cad_image_model import CadImageMotionModel
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import (
|
|
||||||
resolve_image_motion, select_image_motion_frame, image_motion_model_from_dict,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.product import load_product_config
|
|
||||||
from linkerhand_calibration.profiles.observations import compile_parallel_axis_geometry
|
|
||||||
from linkerhand_calibration.runtime.motion_provenance import validate_source_geometry
|
|
||||||
from test_production_image_motion import recorded_image_frames
|
|
||||||
|
|
||||||
|
|
||||||
def product(name):
|
|
||||||
return load_product_config(Path(__file__).resolve().parents[1]/f"config/{name}_right_product.yaml", check_can=False)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(scope="module")
|
|
||||||
def recorded():
|
|
||||||
raw=json.loads(gzip.decompress((Path(__file__).parent/"fixtures/o6_pinky_preparation_160040.json.gz").read_bytes()))
|
|
||||||
frames=recorded_image_frames(raw["frames"])
|
|
||||||
relations=tuple(MotionRelation(**value) for value in raw["relations"])
|
|
||||||
config=product("o6");profile=config.calibration_contract.typed_profile
|
|
||||||
constraints=compile_parallel_axis_geometry(profile,UrdfKinematicModel(config.source_urdf),relations)
|
|
||||||
result=resolve_image_motion(frames,relations,geometry_constraints=constraints,
|
|
||||||
source_urdf_sha256=raw["source_urdf_sha256"])
|
|
||||||
return config,frames,relations,constraints,result
|
|
||||||
|
|
||||||
|
|
||||||
def test_recorded_ambiguity_resolves_with_unchanged_cad_and_original_gates(recorded):
|
|
||||||
config,frames,relations,constraints,result=recorded
|
|
||||||
assert len(frames)==104
|
|
||||||
assert constraints[0].distance_m==pytest.approx(.03699959361344391,abs=1e-12)
|
|
||||||
assert hashlib.sha256(config.source_urdf.read_bytes()).hexdigest()==result.model.source_urdf_sha256
|
|
||||||
free=resolve_image_motion(frames,relations)
|
|
||||||
assert not free.resolved and free.reason=="image_motion_families_not_distinguishable"
|
|
||||||
assert result.resolved,result.reason
|
|
||||||
assert len(result.hypotheses)==4
|
|
||||||
assert all(h.converged and h.training_solver_evaluations<=150 for h in result.hypotheses)
|
|
||||||
assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns)
|
|
||||||
assert result.model.maximum_reprojection_error_px==1.5 and result.model.reprojection_tie_px==.03
|
|
||||||
assert image_motion_model_from_dict(asdict(result.model))==result.model
|
|
||||||
assert isinstance(result.model,CadImageMotionModel)
|
|
||||||
for frame in frames[1::8]:
|
|
||||||
measured=select_image_motion_frame(result.model,frame)
|
|
||||||
assert measured.resolved,measured.reason
|
|
||||||
assert max(q.maximum_frame_rms_px for q in measured.quality)<1.5
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("change", ["distance","axis","source","declared_distance"])
|
|
||||||
def test_changed_geometry_or_source_cannot_become_frozen_evidence(recorded,change):
|
|
||||||
config,_,_,_,result=recorded
|
|
||||||
payload=json.loads(json.dumps(asdict(result.model)))
|
|
||||||
if change=="distance":payload["geometry"][1]["pivot_parent_xyz_m"][0]+=.01
|
|
||||||
elif change=="axis":payload["geometry"][1]["axis_parent_xyz"]=payload["geometry"][0]["axis_parent_xyz"]
|
|
||||||
elif change=="declared_distance":payload["constraints"][0]["distance_m"]+=.01
|
|
||||||
else:payload["source_urdf_sha256"]="f"*64
|
|
||||||
if change!="source":
|
|
||||||
with pytest.raises(ValueError,match="source_axis"):
|
|
||||||
image_motion_model_from_dict(payload)
|
|
||||||
else:
|
|
||||||
from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256
|
|
||||||
record={"kind":"motion_branch_initialization","status":"resolved","frozen_image_model":payload,
|
|
||||||
"image_model_sha256":image_model_sha256(payload)}
|
|
||||||
with pytest.raises(ValueError,match="source_geometry_changed"):
|
|
||||||
validate_source_geometry(config.calibration_contract.typed_profile,config.source_urdf,[record])
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("name", ["g20","l6","o6","o12"])
|
|
||||||
def test_compiled_constraints_remain_invariant_under_independent_joint_zeros(name):
|
|
||||||
config=product(name);profile=config.calibration_contract.typed_profile
|
|
||||||
model=UrdfKinematicModel(config.source_urdf)
|
|
||||||
for task in profile.motion.tasks:
|
|
||||||
relations=tuple(MotionRelation(joint,profile.measurement.measurements[joint].parent_role,
|
|
||||||
profile.measurement.measurements[joint].child_role) for joint in task.joints)
|
|
||||||
for constraint in compile_parallel_axis_geometry(profile,model,relations):
|
|
||||||
angles={joint:.11+.013*i for i,joint in enumerate(model.joints)}
|
|
||||||
transforms=[model.link_transform(joint,zero_offsets=angles,joint_angles=angles,
|
|
||||||
independent_mimic_angles=True) for joint in (constraint.parent_joint,constraint.child_joint)]
|
|
||||||
a,b=transforms
|
|
||||||
axis=a[:3,:3]@model.joints[constraint.parent_joint].axis
|
|
||||||
other=b[:3,:3]@model.joints[constraint.child_joint].axis
|
|
||||||
np.testing.assert_allclose(other,constraint.axis_sign*axis,atol=1e-8)
|
|
||||||
delta=b[:3,3]-a[:3,3]
|
|
||||||
assert np.linalg.norm(delta-axis*(axis@delta))==pytest.approx(constraint.distance_m,abs=1e-10)
|
|
||||||
|
|
||||||
|
|
||||||
def synthetic_frames(sign, hinges=2):
|
|
||||||
frames=[];truth=[]
|
|
||||||
camera=np.array([[2000.,0,800.],[0,2000.,600.],[0,0,1.]])
|
|
||||||
root_r=Rotation.from_euler("xyz",(.2,-.15,.1));root_t=np.array([.0,.0,.8])
|
|
||||||
mount_r=[Rotation.from_euler("xyz",(.13,.22,-.1)),Rotation.from_euler("xyz",(-.2,.1,.3))]
|
|
||||||
mount_t=[np.array([.01,.015,.004]),np.array([.012,-.017,.006])]
|
|
||||||
for i,q in enumerate(np.linspace(0,1.4,49)):
|
|
||||||
first_r=root_r*Rotation.from_rotvec([0,0,q])
|
|
||||||
first_t=root_t+root_r.apply([.005,.012,.008])
|
|
||||||
second_r=first_r*Rotation.from_rotvec([0,0,sign*(.6*q+.15*q*q)])
|
|
||||||
second_t=first_t+first_r.apply([.037,0,.015])
|
|
||||||
actual=[("root",root_r,root_t),("moving",first_r*mount_r[0],first_t+first_r.apply(mount_t[0])),
|
|
||||||
("terminal",second_r*mount_r[1],second_t+second_r.apply(mount_t[1]))]
|
|
||||||
if hinges == 3:
|
|
||||||
third_r = second_r * Rotation.from_rotvec([0, 0, -sign * (.4*q + .12*q*q)])
|
|
||||||
third_t = second_t + second_r.apply([.028, 0, -.005])
|
|
||||||
actual.append(("tip", third_r * Rotation.from_euler("xyz", (.08, -.25, .19)),
|
|
||||||
third_t + third_r.apply([.012, .008, -.002])))
|
|
||||||
observations=[];candidates=[];poses={}
|
|
||||||
for role,r,t in actual:
|
|
||||||
points=(r.apply(square_object_points(.016))+t)@camera.T
|
|
||||||
pixels=points[:,:2]/points[:,2:]
|
|
||||||
observations.append(ImageRoleObservation(role,.016,tuple(map(tuple,pixels))))
|
|
||||||
poses[role]=SquareTagPose(tuple(r.as_quat()),tuple(t),0.)
|
|
||||||
noisy=t+([0,0,.003*np.sin(4*q)] if role!="root" else np.zeros(3))
|
|
||||||
candidates.append(RoleCandidates(role,(SquareTagPose(tuple(r.as_quat()),tuple(noisy),.1),),role=="root"))
|
|
||||||
frames.append(ImageMotionFrame(MotionEvidenceFrame(1000000000+i*50000000,tuple(candidates)),
|
|
||||||
tuple(map(tuple,camera)),tuple(observations)))
|
|
||||||
truth.append(poses)
|
|
||||||
return tuple(frames),truth
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("sign", [-1,1])
|
|
||||||
@pytest.mark.parametrize("hinges", [2, 3])
|
|
||||||
def test_independent_images_recover_known_motion_with_parallel_or_opposite_axes(sign, hinges):
|
|
||||||
frames,truth=synthetic_frames(sign, hinges)
|
|
||||||
relations=(MotionRelation("first","root","moving"),MotionRelation("second","moving","terminal"))
|
|
||||||
constraints=(ParallelAxisGeometry("first","second",sign,.037),)
|
|
||||||
if hinges == 3:
|
|
||||||
relations += (MotionRelation("third", "terminal", "tip"),)
|
|
||||||
constraints += (ParallelAxisGeometry("second", "third", -1, .028),)
|
|
||||||
result=resolve_image_motion(frames,relations,geometry_constraints=constraints,
|
|
||||||
source_urdf_sha256="a"*64)
|
|
||||||
assert result.resolved,result.reason
|
|
||||||
for i in range(1,len(frames),8):
|
|
||||||
selected=select_image_motion_frame(result.model,frames[i])
|
|
||||||
assert selected.resolved,selected.reason
|
|
||||||
for value in selected.selections:
|
|
||||||
actual=truth[i][value.role]
|
|
||||||
np.testing.assert_allclose(value.pose.translation_xyz_m,actual.translation_xyz_m,atol=1e-6)
|
|
||||||
assert (Rotation.from_quat(actual.quaternion_xyzw).inv()*Rotation.from_quat(value.pose.quaternion_xyzw)).magnitude()<1e-5
|
|
||||||
|
|
||||||
|
|
||||||
def test_geometry_incompatible_with_images_cannot_be_forced_through_acceptance():
|
|
||||||
frames, _ = synthetic_frames(1)
|
|
||||||
relations = (MotionRelation("first", "root", "moving"), MotionRelation("second", "moving", "terminal"))
|
|
||||||
result = resolve_image_motion(frames, relations,
|
|
||||||
geometry_constraints=(ParallelAxisGeometry("first", "second", 1, .08),),
|
|
||||||
source_urdf_sha256="a"*64)
|
|
||||||
assert not result.resolved
|
|
||||||
assert all(hypothesis.reason == "image_motion_reprojection_failed" for hypothesis in result.hypotheses)
|
|
||||||
@@ -1,177 +0,0 @@
|
|||||||
import copy
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.calibrated_joint_state_bridge import (
|
|
||||||
G20_COMMAND_NAMES,
|
|
||||||
G20_URDF_JOINT_NAMES,
|
|
||||||
CalibratedCommandMapper,
|
|
||||||
default_input_topic,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.full_hand import (
|
|
||||||
JointCurveFit,
|
|
||||||
build_compact_payload,
|
|
||||||
get_hand_calibration_profile,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.urdf_zero import (
|
|
||||||
get_zero_calibration_profile,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _payload(side: str = "right") -> dict:
|
|
||||||
profile = get_hand_calibration_profile(side)
|
|
||||||
curve = [round((255 - command) * 0.001, 8) for command in range(256)]
|
|
||||||
joints = {}
|
|
||||||
for name, spec in profile.joint_specs.items():
|
|
||||||
joint = {
|
|
||||||
"motor_index": spec.motor_index,
|
|
||||||
"angle_rad": list(curve),
|
|
||||||
}
|
|
||||||
if spec.active:
|
|
||||||
joint["zero_command_u8"] = 255
|
|
||||||
joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
|
|
||||||
else:
|
|
||||||
joint["passive"] = True
|
|
||||||
if spec.source_joint is not None:
|
|
||||||
joint["source_joint"] = spec.source_joint
|
|
||||||
joints[name] = joint
|
|
||||||
return {
|
|
||||||
"schema_version": 4,
|
|
||||||
"model": "G20",
|
|
||||||
"side": side,
|
|
||||||
"serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
|
|
||||||
"angle_unit": "rad",
|
|
||||||
"command_range": [0, 255],
|
|
||||||
"baseline_command_u8": [255] * 20,
|
|
||||||
"joints": joints,
|
|
||||||
"quality": {
|
|
||||||
"passed": True,
|
|
||||||
"validation_mae_rad": 0.01,
|
|
||||||
"validation_p95_rad": 0.02,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
|
|
||||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
|
||||||
command = list(range(20))
|
|
||||||
result = dict(
|
|
||||||
zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
|
|
||||||
)
|
|
||||||
|
|
||||||
assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
|
|
||||||
assert result["thumb_cmc_roll"] == pytest.approx(0.250)
|
|
||||||
assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
|
|
||||||
assert result["thumb_mcp"] == pytest.approx(0.240)
|
|
||||||
assert result["thumb_ip"] == pytest.approx(0.240)
|
|
||||||
assert result["pinky_pip"] == pytest.approx(0.236)
|
|
||||||
assert result["pinky_dip"] == pytest.approx(0.236)
|
|
||||||
|
|
||||||
|
|
||||||
def test_mapper_uses_names_instead_of_message_order() -> None:
|
|
||||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
|
||||||
command = list(range(20))
|
|
||||||
names = list(reversed(G20_COMMAND_NAMES))
|
|
||||||
positions = list(reversed(command))
|
|
||||||
|
|
||||||
assert mapper.map_positions(positions, names) == mapper.map_positions(command)
|
|
||||||
|
|
||||||
|
|
||||||
def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
|
|
||||||
with pytest.raises(ValueError, match="does not match"):
|
|
||||||
CalibratedCommandMapper(_payload("left"), expected_side="right")
|
|
||||||
|
|
||||||
payload = copy.deepcopy(_payload())
|
|
||||||
payload["quality"]["passed"] = False
|
|
||||||
with pytest.raises(ValueError, match="quality.passed"):
|
|
||||||
CalibratedCommandMapper(payload, expected_side="right")
|
|
||||||
|
|
||||||
|
|
||||||
def test_mapper_rejects_incomplete_named_command() -> None:
|
|
||||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
|
||||||
with pytest.raises(ValueError, match="missing named channels"):
|
|
||||||
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
|
|
||||||
|
|
||||||
|
|
||||||
def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
|
|
||||||
profile = get_hand_calibration_profile("right", "g20_right_15")
|
|
||||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
|
||||||
baseline = [255] * 20
|
|
||||||
baseline[6:10] = [127] * 4
|
|
||||||
fits = {}
|
|
||||||
diagnostics = {}
|
|
||||||
for name, spec in profile.joint_specs.items():
|
|
||||||
zero_command = baseline[spec.motor_index]
|
|
||||||
average = tuple(
|
|
||||||
0.001 * (zero_command - value) for value in range(256)
|
|
||||||
)
|
|
||||||
decreasing = tuple(1.1 * value for value in average)
|
|
||||||
increasing = tuple(0.9 * value for value in average)
|
|
||||||
fits[name] = JointCurveFit(
|
|
||||||
average,
|
|
||||||
decreasing,
|
|
||||||
increasing,
|
|
||||||
{},
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
|
|
||||||
)
|
|
||||||
diagnostics[name] = {
|
|
||||||
"cycle_travel_rad": [0.25] * 4,
|
|
||||||
"cycle_travel_range_rad": 0.0,
|
|
||||||
"baseline_hysteresis_by_cycle_rad": [0.0] * 4,
|
|
||||||
"holdout_cycle": 3,
|
|
||||||
"holdout_cycle_mae_rad": 0.0,
|
|
||||||
"holdout_cycle_max_rad": 0.0,
|
|
||||||
}
|
|
||||||
payload = build_compact_payload(
|
|
||||||
serial_number="TEST_RIGHT_V5",
|
|
||||||
measured_fits=fits,
|
|
||||||
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
|
|
||||||
validation_errors_rad=[0.0],
|
|
||||||
passed=True,
|
|
||||||
baseline=baseline,
|
|
||||||
side="right",
|
|
||||||
layout_id="g20_right_15",
|
|
||||||
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
|
|
||||||
zero_cycle_offsets_rad={
|
|
||||||
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
|
|
||||||
},
|
|
||||||
zero_observers=zero.offset_observer_joint,
|
|
||||||
artifact_hashes={
|
|
||||||
"source_urdf_sha256": "0" * 64,
|
|
||||||
"camera_extrinsics_sha256": "1" * 64,
|
|
||||||
"corrected_urdf_sha256": "2" * 64,
|
|
||||||
},
|
|
||||||
cross_view_roll_metrics={
|
|
||||||
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
|
|
||||||
for finger in ("index", "middle", "ring", "pinky")
|
|
||||||
},
|
|
||||||
joint_dynamic_diagnostics=diagnostics,
|
|
||||||
zero_geometry_diagnostics={
|
|
||||||
"training_cycles": (0, 1, 2),
|
|
||||||
"validation_cycle": 3,
|
|
||||||
"axis_line_rms_m": 0.0,
|
|
||||||
"validation_line_error_by_joint_m": {},
|
|
||||||
},
|
|
||||||
)
|
|
||||||
mapper = CalibratedCommandMapper(payload, expected_side="right")
|
|
||||||
command = [255.0] * 20
|
|
||||||
command[0] = 100.0
|
|
||||||
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
|
|
||||||
|
|
||||||
assert mapper.input_domain == "command_u8"
|
|
||||||
assert mapper.layout_id == "g20_right_19"
|
|
||||||
# midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
|
|
||||||
assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
|
|
||||||
assert payload["joints"]["pinky_dip"]["passive"] is True
|
|
||||||
assert "source_joint" not in payload["joints"]["pinky_dip"]
|
|
||||||
|
|
||||||
|
|
||||||
def test_default_topics_use_the_g20_sdk_namespace() -> None:
|
|
||||||
assert default_input_topic("right", "command_u8") == (
|
|
||||||
"/g20/cb_right_hand_control_cmd"
|
|
||||||
)
|
|
||||||
assert default_input_topic("right", "feedback_u8") == (
|
|
||||||
"/g20/cb_right_hand_state"
|
|
||||||
)
|
|
||||||
@@ -1,161 +0,0 @@
|
|||||||
"""Measured amplitude may replace nominal CAD limits, never safety evidence."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
import hashlib
|
|
||||||
from pathlib import Path
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.profile import JointLimitPolicy, validate_profile
|
|
||||||
from linkerhand_calibration.core.domain.result import JointMapping
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.core.urdf.acceptance import validate_artifact_structure
|
|
||||||
from linkerhand_calibration.core.urdf.limits import MotionRangeEvidence, compile_motion_ranges, validate_range_evidence
|
|
||||||
from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.export import write_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.loader import load_hand_profile
|
|
||||||
|
|
||||||
|
|
||||||
SOURCE = Path(__file__).resolve().parents[1] / "urdf/o6_right/linkerhand_o6_right.urdf"
|
|
||||||
PITCH, IP = "rh_thumb_cmc_pitch", "rh_thumb_ip"
|
|
||||||
|
|
||||||
|
|
||||||
def evidence(name, upper, *, delta=0., safety=None):
|
|
||||||
original = UrdfKinematicModel(SOURCE).joints[name]
|
|
||||||
return MotionRangeEvidence(name, name, True, (original.lower, original.upper),
|
|
||||||
(0., upper), (0., upper), (0., upper), delta, 255., (0, 1, 2),
|
|
||||||
("front:100", "front:200"), ("front:10",), "calibrated_motion_envelope",
|
|
||||||
"fixture nominal CAD amplitude policy", safety, "fixture independent hardware bound" if safety else "")
|
|
||||||
|
|
||||||
|
|
||||||
def plan(evidence_by_joint=None, *, authorized=None):
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
return build_standard_correction_plan(source_urdf=SOURCE,
|
|
||||||
source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(),
|
|
||||||
zero_offsets_rad={PITCH: 0., IP: 0.},
|
|
||||||
measured_ranges_output_rad={PITCH: (0., .60), IP: (0., 1.16)},
|
|
||||||
range_evidence=evidence_by_joint,
|
|
||||||
authorized_fields=profile.urdf_authorized_fields if authorized is None else authorized)
|
|
||||||
|
|
||||||
|
|
||||||
def test_nominal_range_requires_explicit_training_evidence_and_authorization(tmp_path):
|
|
||||||
with pytest.raises(ValueError, match="exceeds source mechanical range"):
|
|
||||||
plan()
|
|
||||||
entries = {PITCH: evidence(PITCH, .60), IP: evidence(IP, 1.16)}
|
|
||||||
original = SOURCE.read_bytes()
|
|
||||||
result = plan(entries)
|
|
||||||
output = tmp_path / "corrected.urdf"
|
|
||||||
result.write(SOURCE, output)
|
|
||||||
before, after = UrdfKinematicModel(SOURCE), UrdfKinematicModel(output)
|
|
||||||
assert SOURCE.read_bytes() == original
|
|
||||||
assert after.joints[IP].upper == 1.16
|
|
||||||
assert (after.joints[IP].origin == before.joints[IP].origin).all()
|
|
||||||
assert result.range_evidence[IP].training_cycles == (0, 1, 2)
|
|
||||||
rights = dict(load_bundled_hand_profile("o6_right_8").urdf_authorized_fields)
|
|
||||||
rights[IP] = frozenset({"mimic.multiplier", "mimic.offset"})
|
|
||||||
with pytest.raises(ValueError, match="absent from Profile authorization"):
|
|
||||||
plan(entries, authorized=rights)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("mutation,reason", [
|
|
||||||
({"training_cycles": (0, 1, 2, 3)}, "invalid_training"),
|
|
||||||
({"training_sample_ids": ()}, "invalid_training"),
|
|
||||||
({"reference_sample_ids": ()}, "invalid_training"),
|
|
||||||
({"output_limits_rad": (0., 1.17)}, "differs_from_training"),
|
|
||||||
({"independent_safety_limits_cad_rad": (0., 1.1), "safety_evidence_source": "known hardware"}, "independent_safety"),
|
|
||||||
({"independent_safety_limits_cad_rad": (0., 2.), "safety_evidence_source": ""}, "independent_safety"),
|
|
||||||
])
|
|
||||||
def test_evidence_cannot_use_holdout_pad_or_override_independent_safety(mutation, reason):
|
|
||||||
item = replace(evidence(IP, 1.16), **mutation)
|
|
||||||
with pytest.raises(ValueError, match=reason):
|
|
||||||
validate_range_evidence(IP, UrdfKinematicModel(SOURCE).joints[IP], 0., (0., 1.16), item)
|
|
||||||
|
|
||||||
|
|
||||||
def test_independent_safety_is_checked_in_cad_coordinates_once():
|
|
||||||
original = UrdfKinematicModel(SOURCE).joints[PITCH]
|
|
||||||
item = evidence(PITCH, .6, delta=.1, safety=(.09, .71))
|
|
||||||
validate_range_evidence(PITCH, original, .1, (0., .6), item)
|
|
||||||
with pytest.raises(ValueError, match="independent_safety"):
|
|
||||||
validate_range_evidence(PITCH, original, .1, (0., .6),
|
|
||||||
replace(item, independent_safety_limits_cad_rad=(.09, .69)))
|
|
||||||
|
|
||||||
|
|
||||||
def test_final_readback_needs_trusted_exact_envelope_not_payload_permission(tmp_path):
|
|
||||||
import xml.etree.ElementTree as ET
|
|
||||||
entries = {PITCH: evidence(PITCH, .60), IP: evidence(IP, 1.16)}
|
|
||||||
result = plan(entries)
|
|
||||||
output = tmp_path / "corrected.urdf"
|
|
||||||
result.write(SOURCE, output)
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint
|
|
||||||
model = UrdfKinematicModel(output)
|
|
||||||
payload = {"format": "unified_calibration_v3", "schema_version": 3,
|
|
||||||
"input_unit": "u8", "baseline_command": [255]*6,
|
|
||||||
"joints": {name: {"sdk_channel": channel_for_joint(profile, name),
|
|
||||||
"angle_rad": [joint.upper*(255-code)/255 for code in range(256)]}
|
|
||||||
for name, joint in model.joints.items() if joint.kind != "fixed"}}
|
|
||||||
args = dict(source_urdf=SOURCE, corrected_urdf=output,
|
|
||||||
source_sha256=result.source_sha256, authorized_fields=profile.urdf_authorized_fields,
|
|
||||||
payload=payload, zero_offsets_rad={PITCH: 0., IP: 0.})
|
|
||||||
assert validate_artifact_structure(**args, range_evidence=entries)
|
|
||||||
with pytest.raises(ValueError, match="exceeds source mechanical range"):
|
|
||||||
validate_artifact_structure(**args)
|
|
||||||
tree = ET.parse(output)
|
|
||||||
tree.find(f".//joint[@name='{PITCH}']/limit").set("upper", ".601")
|
|
||||||
tree.write(output)
|
|
||||||
with pytest.raises(ValueError, match="differs_from_training"):
|
|
||||||
validate_artifact_structure(**args, range_evidence=entries)
|
|
||||||
|
|
||||||
|
|
||||||
def test_mimic_does_not_expand_measured_envelope_to_make_it_pass():
|
|
||||||
with pytest.raises(ValueError, match="differs_from_training"):
|
|
||||||
plan({PITCH: evidence(PITCH, .6), IP: evidence(IP, 1.09)})
|
|
||||||
# A validly supported 1.09 rad child cannot accommodate the unchanged
|
|
||||||
# standard mimic's 1.86 * .6 range; the plan must reject, never add padding.
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
with pytest.raises(ValueError, match="mimic reachable range exceeds"):
|
|
||||||
build_standard_correction_plan(source_urdf=SOURCE,
|
|
||||||
source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(),
|
|
||||||
zero_offsets_rad={PITCH: 0., IP: 0.},
|
|
||||||
measured_ranges_output_rad={PITCH: (0., .6), IP: (0., 1.09)},
|
|
||||||
range_evidence={PITCH: evidence(PITCH, .6), IP: evidence(IP, 1.09)},
|
|
||||||
authorized_fields=profile.urdf_authorized_fields)
|
|
||||||
|
|
||||||
|
|
||||||
def test_profile_roundtrip_keeps_explicit_policy_and_dip_origin_protection(tmp_path):
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
output = tmp_path / "profile.yaml"
|
|
||||||
write_hand_profile(profile, output)
|
|
||||||
assert load_hand_profile(output) == profile
|
|
||||||
assert len(profile.urdf_limit_policies) == 11
|
|
||||||
assert all("origin.rpy" not in profile.urdf_authorized_fields[name] for name in profile.zero.passive_joints)
|
|
||||||
policies = dict(profile.urdf_limit_policies)
|
|
||||||
policies[IP] = JointLimitPolicy("calibrated_motion_envelope", "source policy", (0., 2.), "")
|
|
||||||
with pytest.raises(ValueError, match="safety bounds"):
|
|
||||||
validate_profile(replace(profile, urdf_limit_policies=policies))
|
|
||||||
|
|
||||||
|
|
||||||
def test_transferred_envelope_keeps_donor_training_and_ignores_holdout_errors():
|
|
||||||
donor, target = "rh_pinky_dip", "rh_index_dip"
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
profile = replace(profile, urdf_limit_policies={target: profile.urdf_limit_policies[target]})
|
|
||||||
def mapping(domain):
|
|
||||||
return JointMapping(target, 2, domain, (0., 255.), (1.5, 0.), (1.51, 0.), (1.49, 0.), donor)
|
|
||||||
fit = SimpleNamespace(curves={donor: SimpleNamespace(circle={"training_cycles": [0, 1, 2],
|
|
||||||
"training_sample_ids": ["side:100", "side:200"]})},
|
|
||||||
joint_zero_references={donor: SimpleNamespace(baseline_command=255.,
|
|
||||||
samples=[SimpleNamespace(view="side", image_stamp_ns=10)])},
|
|
||||||
command_holdout_metrics={donor: {"training_cycles": [4], "holdout_cycle": 5,
|
|
||||||
"training_sample_ids": ["side:300", "side:400"], "maximum_deg": 2}},
|
|
||||||
output_mappings={target: mapping("feedback_u8")}, command_mappings={target: mapping("command_u8")},
|
|
||||||
zero_offsets_rad={target: 0.})
|
|
||||||
model = UrdfKinematicModel(SOURCE)
|
|
||||||
before = compile_motion_ranges(profile, model, fit)[target]
|
|
||||||
fit.command_holdout_metrics[donor]["maximum_deg"] = 100
|
|
||||||
after = compile_motion_ranges(profile, model, fit)[target]
|
|
||||||
assert before == after
|
|
||||||
assert before.source_joint == donor and before.independently_measured is False
|
|
||||||
assert before.output_limits_rad == (0., 1.51)
|
|
||||||
assert before.training_sample_ids == ("side:100", "side:200")
|
|
||||||
@@ -1,294 +0,0 @@
|
|||||||
"""Session/observation/publication races exercised through real coordinator ports."""
|
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
from dataclasses import replace
|
|
||||||
import json
|
|
||||||
from threading import Event
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.runtime.artifacts.controller import FinalizationController
|
|
||||||
from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput
|
|
||||||
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady", "joint_zero", "fit"])
|
|
||||||
def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monkeypatch, change):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
if change != "start":
|
|
||||||
assert host.start().success
|
|
||||||
host.execution.session.phase = Phase.SWEEP
|
|
||||||
host._motion = MotionCommand(change if change in {"steady", "joint_zero"} else "sweep",
|
|
||||||
host.profile.command.baseline_values, 1., task_key=host.profile.motion.tasks[0].key,
|
|
||||||
command_index=0, cycle=0, direction="decreasing") if change != "start" else None
|
|
||||||
host._steady_capture_after_ns = host.ports.clock_ns()
|
|
||||||
|
|
||||||
def consume(*args, **kwargs):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
return [{"kind": "late_sample"}], None
|
|
||||||
|
|
||||||
monkeypatch.setattr(host.capture, "consume", consume)
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(host.receive_detections, DetectionInput(
|
|
||||||
host.profile.vision.view_names[0], host.ports.clock_ns(), ()))
|
|
||||||
assert entered.wait(2)
|
|
||||||
# These calls must complete while the expensive pose solve is blocked.
|
|
||||||
if change == "start":
|
|
||||||
assert host.start().success
|
|
||||||
elif change == "pause":
|
|
||||||
host._pause("test_visual_fault")
|
|
||||||
elif change == "abort":
|
|
||||||
assert host.abort().success
|
|
||||||
elif change == "motion":
|
|
||||||
with host.step_data_lock:
|
|
||||||
host._motion = replace(host._motion, cycle=1)
|
|
||||||
elif change == "fit":
|
|
||||||
monkeypatch.setattr(host.finalization, "start", lambda _: None)
|
|
||||||
with host.step_data_lock:
|
|
||||||
host.execution.session.phase = Phase.FIT
|
|
||||||
host._finalize()
|
|
||||||
else:
|
|
||||||
with host.step_data_lock:
|
|
||||||
host._steady_capture_after_ns = None
|
|
||||||
release.set()
|
|
||||||
future.result(timeout=2)
|
|
||||||
assert [row["kind"] for row in host.raw_records] == ["session_start"]
|
|
||||||
assert host._unit_rows == []
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def begin_fit(host, clock):
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
clock.now += .01
|
|
||||||
host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns())
|
|
||||||
host.execution.session.phase = Phase.FIT
|
|
||||||
host.last_command = tuple(host.profile.command.baseline_values)
|
|
||||||
host.tick()
|
|
||||||
assert host.finalization.started
|
|
||||||
|
|
||||||
|
|
||||||
def completed_fit(**kwargs):
|
|
||||||
assert kwargs["publish"] is False
|
|
||||||
for event in ("fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"):
|
|
||||||
kwargs["phase_changed"](event)
|
|
||||||
return ({"format": "unified_calibration_v2", "joints": {}}, None,
|
|
||||||
SimpleNamespace(path=kwargs["session_dir"]/"corrected.urdf", staged_release="staged"))
|
|
||||||
|
|
||||||
|
|
||||||
def replace_finalizer(host, finalizer):
|
|
||||||
host.finalization.close()
|
|
||||||
host.finalization = FinalizationController(host.profile, host.parameters.session_dir, finalizer=finalizer)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"])
|
|
||||||
def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp_path, model):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model=model)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
def fit(**kwargs):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
raise ValueError("synthetic_fit_failure")
|
|
||||||
try:
|
|
||||||
replace_finalizer(host, fit)
|
|
||||||
begin_fit(host, clock)
|
|
||||||
assert entered.wait(2)
|
|
||||||
for _ in range(10):
|
|
||||||
clock.now += .01
|
|
||||||
host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
assert host.snapshot().calibration.phase == "FIT"
|
|
||||||
assert len(host.state_history) == 11
|
|
||||||
release.set()
|
|
||||||
with pytest.raises(ValueError, match="synthetic_fit_failure"):
|
|
||||||
host.finalization.worker._future.result(timeout=2)
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase == Phase.FAILED
|
|
||||||
assert "fit_or_publication_failed:synthetic_fit_failure" == host.reason
|
|
||||||
diagnostic = json.loads((host.parameters.session_dir / "failure_diagnostic.json").read_text())
|
|
||||||
assert diagnostic["stage"] == "FIT"
|
|
||||||
assert diagnostic["raw_samples"] == str(host.raw_path)
|
|
||||||
assert diagnostic["reason"] == "synthetic_fit_failure"
|
|
||||||
assert 'ValueError: synthetic_fit_failure' in diagnostic["traceback"]
|
|
||||||
rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()]
|
|
||||||
assert rows[-1]["kind"] == "finalization_failed"
|
|
||||||
assert not any(row["kind"] == "paused" for row in rows)
|
|
||||||
assert host.snapshot().calibration.outputs["failure_diagnostic"].endswith("failure_diagnostic.json")
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
# Terminal failure must survive late sensor events and operator cleanup.
|
|
||||||
host._pause("late_observation_failure")
|
|
||||||
assert not host.abort().success
|
|
||||||
assert host.execution.session.phase == Phase.FAILED
|
|
||||||
count = len(clock.positions)
|
|
||||||
host.tick()
|
|
||||||
assert len(clock.positions) == count
|
|
||||||
assert all(command == tuple(host.profile.command.baseline_values) for command in clock.positions)
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_finalization_protocol_failure_is_durable_and_cannot_publish(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
commits = []
|
|
||||||
def invalid_order(**kwargs):
|
|
||||||
kwargs["phase_changed"]("urdf_validated")
|
|
||||||
return ({}, None, SimpleNamespace(staged_release="must_not_commit"))
|
|
||||||
try:
|
|
||||||
replace_finalizer(host, invalid_order)
|
|
||||||
monkeypatch.setattr(ArtifactPublisher, "commit", lambda _, staged: commits.append(staged))
|
|
||||||
begin_fit(host, clock)
|
|
||||||
host.finalization.worker._future.result(timeout=2)
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase == Phase.FAILED
|
|
||||||
assert host.reason.startswith("finalization_protocol_error:")
|
|
||||||
assert not commits
|
|
||||||
diagnostic = json.loads((host.parameters.session_dir / "failure_diagnostic.json").read_text())
|
|
||||||
assert diagnostic["state"] == "FAILED" and diagnostic["stage"] == "FIT"
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_fit_uses_closed_capture_while_feedback_and_abort_remain_live(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
def fit(**kwargs):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
return completed_fit(**kwargs)
|
|
||||||
try:
|
|
||||||
replace_finalizer(host, fit)
|
|
||||||
begin_fit(host, clock)
|
|
||||||
assert entered.wait(2)
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
view = host.required_observation_views[0]
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda *a, **k: pytest.fail("capture is closed"))
|
|
||||||
monkeypatch.setattr(host.cameras, "accept", lambda *a, **k: pytest.fail("camera evidence is frozen"))
|
|
||||||
host.receive_detections(DetectionInput(view, host.ports.clock_ns(), ()))
|
|
||||||
host.receive_camera_info(view, None)
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
assert host.execution.session.phase == Phase.FIT
|
|
||||||
clock.now += .02
|
|
||||||
host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns())
|
|
||||||
assert host.state_history[-1].stamp_ns == host.ports.clock_ns()
|
|
||||||
assert host.abort().success
|
|
||||||
assert host.execution.session.phase == Phase.ABORTED
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("abort_first", [True, False])
|
|
||||||
def test_abort_and_artifact_commit_are_mutually_exclusive(tmp_path, monkeypatch, abort_first):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
committing, release, abort_entered, abort_done = Event(), Event(), Event(), Event()
|
|
||||||
commits = []
|
|
||||||
def commit(publisher, staged):
|
|
||||||
committing.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
commits.append(staged)
|
|
||||||
monkeypatch.setattr(ArtifactPublisher, "commit", commit)
|
|
||||||
try:
|
|
||||||
replace_finalizer(host, completed_fit)
|
|
||||||
begin_fit(host, clock)
|
|
||||||
host.finalization.worker._future.result(timeout=2)
|
|
||||||
if abort_first:
|
|
||||||
assert host.abort().success
|
|
||||||
host.tick()
|
|
||||||
assert not commits
|
|
||||||
assert host.execution.session.phase == Phase.ABORTED
|
|
||||||
else:
|
|
||||||
with ThreadPoolExecutor(2) as pool:
|
|
||||||
tick = pool.submit(host.tick)
|
|
||||||
assert committing.wait(2)
|
|
||||||
def abort():
|
|
||||||
abort_entered.set()
|
|
||||||
result = host.abort()
|
|
||||||
abort_done.set()
|
|
||||||
return result
|
|
||||||
aborted = pool.submit(abort)
|
|
||||||
assert abort_entered.wait(2)
|
|
||||||
assert not abort_done.wait(.02)
|
|
||||||
release.set()
|
|
||||||
tick.result(timeout=2)
|
|
||||||
assert not aborted.result(timeout=2).success
|
|
||||||
assert host.execution.session.phase == Phase.COMPLETE
|
|
||||||
assert host.final_json.endswith("o6_right_OFFLINE_COORDINATOR_partial_calibration.json")
|
|
||||||
assert host.final_urdf.endswith("corrected.urdf")
|
|
||||||
host.tick()
|
|
||||||
assert commits == ["staged"]
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_finalizer_last_events_are_drained_after_future_is_ready(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
commits = []
|
|
||||||
try:
|
|
||||||
replace_finalizer(host, completed_fit)
|
|
||||||
begin_fit(host, clock)
|
|
||||||
host.finalization.worker._future.result(timeout=2)
|
|
||||||
events = tuple(host.finalization._events.get() for _ in range(4))
|
|
||||||
original = host.finalization.worker.finish_if_ready
|
|
||||||
def finish(commit):
|
|
||||||
# Simulate worker completion between poll's first drain and done().
|
|
||||||
for event in events:
|
|
||||||
host.finalization._events.put(event)
|
|
||||||
return original(commit)
|
|
||||||
monkeypatch.setattr(host.finalization.worker, "finish_if_ready", finish)
|
|
||||||
monkeypatch.setattr(ArtifactPublisher, "commit", lambda _, staged: commits.append(staged))
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase == Phase.COMPLETE
|
|
||||||
assert commits == ["staged"]
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_paused_session_does_not_issue_motion_when_timer_acquires_lock(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
tick_entered = Event()
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
with host.step_data_lock:
|
|
||||||
def tick():
|
|
||||||
tick_entered.set()
|
|
||||||
host.tick()
|
|
||||||
future = pool.submit(tick)
|
|
||||||
assert tick_entered.wait(2)
|
|
||||||
host._pause("test_visual_fault")
|
|
||||||
count = len(clock.positions)
|
|
||||||
future.result(timeout=2)
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert len(clock.positions) == count
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_sdk_binding_uses_supplied_clock_for_transient_health_recovery(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model="o12")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.sdk_adapter.health().safe
|
|
||||||
clock.health_receiver("malformed")
|
|
||||||
assert not host.sdk_adapter.health().active_faults
|
|
||||||
clock.now += 1.01
|
|
||||||
assert host.sdk_adapter.health().active_faults == ("health_report_unparseable",)
|
|
||||||
clock.health_receiver('{"position_mode": true, "active_faults": []}')
|
|
||||||
host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values,
|
|
||||||
host.ports.clock_ns())
|
|
||||||
assert host.sdk_adapter.health().safe
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,222 +0,0 @@
|
|||||||
"""Pre-Start evidence must not confuse protected files with physical calibration."""
|
|
||||||
|
|
||||||
import json
|
|
||||||
from dataclasses import FrozenInstanceError, asdict, replace
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.extrinsics import (
|
|
||||||
CameraCalibrationIdentity,
|
|
||||||
CameraExtrinsics,
|
|
||||||
camera_info_fingerprint,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.runtime.camera_preflight import CameraPreflight
|
|
||||||
from linkerhand_calibration.runtime.cameras import CameraObservations
|
|
||||||
from linkerhand_calibration.runtime.inputs import CameraModelInput
|
|
||||||
from linkerhand_calibration.runtime.snapshot import CameraReadiness
|
|
||||||
from linkerhand_calibration.runtime.status import render_status_zh
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
|
|
||||||
|
|
||||||
def preflight_rows(host):
|
|
||||||
path = host.parameters.session_dir / "raw_samples.jsonl"
|
|
||||||
return [row for line in path.read_text().splitlines()
|
|
||||||
if (row := json.loads(line)).get("kind") == "camera_preflight"]
|
|
||||||
|
|
||||||
|
|
||||||
def saved_preflight(host):
|
|
||||||
return json.loads((host.parameters.session_dir / "camera_preflight.json").read_text())
|
|
||||||
|
|
||||||
|
|
||||||
def assert_physical_calibration_unverified(report):
|
|
||||||
for name in ("intrinsics", "extrinsics"):
|
|
||||||
assert report[name]["status"] == "unverified"
|
|
||||||
assert report[name]["reason"]
|
|
||||||
assert report[name]["suggestion"]
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("detections_ready", [False, True])
|
|
||||||
def test_valid_camera_info_only_proves_configuration(detections_ready):
|
|
||||||
report = CameraPreflight.from_readiness({
|
|
||||||
"arbitrary_camera_name": CameraReadiness(True, detections_ready, ""),
|
|
||||||
})
|
|
||||||
payload = report.as_dict()
|
|
||||||
assert payload["configuration"]["arbitrary_camera_name"]["status"] == "passed"
|
|
||||||
assert_physical_calibration_unverified(payload)
|
|
||||||
with pytest.raises(FrozenInstanceError):
|
|
||||||
report.intrinsics = None
|
|
||||||
|
|
||||||
|
|
||||||
def test_configuration_failure_takes_precedence_over_old_ready_flags():
|
|
||||||
payload = CameraPreflight.from_readiness({
|
|
||||||
"front": CameraReadiness(True, True, "camera_info_does_not_match_extrinsics"),
|
|
||||||
"side": CameraReadiness(False, True, ""),
|
|
||||||
"top": CameraReadiness(True, False, ""),
|
|
||||||
}).as_dict()
|
|
||||||
assert payload["configuration"]["front"]["status"] == "failed"
|
|
||||||
assert "camera_info_does_not_match_extrinsics" in payload["configuration"]["front"]["reason"]
|
|
||||||
assert payload["configuration"]["front"]["suggestion"]
|
|
||||||
assert payload["configuration"]["side"]["status"] == "unverified"
|
|
||||||
assert payload["configuration"]["top"]["status"] == "passed"
|
|
||||||
assert_physical_calibration_unverified(payload)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("model", ["o6", "l6", "o12", "g20"])
|
|
||||||
def test_all_models_record_start_evidence_without_requiring_visible_finger_tags(tmp_path, model):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model=model)
|
|
||||||
try:
|
|
||||||
# The detector stream is alive, but no hand Tag or historical pose is supplied.
|
|
||||||
ready(host, clock)
|
|
||||||
report = host.snapshot().as_dict()["camera_preflight"]
|
|
||||||
assert all(item["status"] == "passed" for item in report["configuration"].values())
|
|
||||||
assert_physical_calibration_unverified(report)
|
|
||||||
before_start = saved_preflight(host)
|
|
||||||
assert before_start["kind"] == "camera_preflight"
|
|
||||||
assert before_start["stage"] == "before_start"
|
|
||||||
assert before_start["stamp_ns"] == host.ports.clock_ns()
|
|
||||||
assert before_start["protected_inputs"] == dict(host.parameters.protected_inputs)
|
|
||||||
assert before_start["expected_cameras"] == {
|
|
||||||
view: asdict(identity) for view, identity in host.parameters.extrinsics.cameras.items()
|
|
||||||
}
|
|
||||||
for key, value in report.items():
|
|
||||||
assert before_start[key] == value
|
|
||||||
assert not clock.positions
|
|
||||||
|
|
||||||
assert host.start().success
|
|
||||||
at_start = saved_preflight(host)
|
|
||||||
assert at_start["stage"] == "at_start"
|
|
||||||
assert_physical_calibration_unverified(at_start)
|
|
||||||
assert preflight_rows(host)[-1] == at_start
|
|
||||||
|
|
||||||
# Later freshness changes cannot rewrite the historical Start evidence.
|
|
||||||
clock.now += 3.0
|
|
||||||
assert host.snapshot().as_dict()["camera_preflight"] == report
|
|
||||||
assert saved_preflight(host) == at_start
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_unchanged_preflight_does_not_fill_the_sample_journal(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
original = preflight_rows(host)
|
|
||||||
for _ in range(5):
|
|
||||||
clock.now += 0.05
|
|
||||||
host.tick()
|
|
||||||
host.snapshot()
|
|
||||||
assert preflight_rows(host) == original
|
|
||||||
|
|
||||||
view = host.profile.vision.view_names[0]
|
|
||||||
host.cameras.errors[view] = "camera_info_does_not_match_extrinsics"
|
|
||||||
host.tick()
|
|
||||||
assert len(preflight_rows(host)) == len(original) + 1
|
|
||||||
assert saved_preflight(host)["configuration"][view]["status"] == "failed"
|
|
||||||
host.tick()
|
|
||||||
assert len(preflight_rows(host)) == len(original) + 1
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["camera_info_stale", "camera_info_mismatch", "detector_stale"])
|
|
||||||
def test_start_rechecks_live_devices_and_records_rejection_before_motion(tmp_path, fault):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
view = host.profile.vision.view_names[0]
|
|
||||||
if fault == "camera_info_stale":
|
|
||||||
host.cameras.info_received_at[view] = clock.now - 2.01
|
|
||||||
expected = "unverified"
|
|
||||||
elif fault == "camera_info_mismatch":
|
|
||||||
host.cameras.errors[view] = "camera_info_does_not_match_extrinsics"
|
|
||||||
expected = "failed"
|
|
||||||
else:
|
|
||||||
host.cameras.detections_received_at[view] = clock.now - 2.01
|
|
||||||
expected = "passed"
|
|
||||||
positions, settings = list(clock.positions), list(clock.settings)
|
|
||||||
assert not host.start().success
|
|
||||||
assert not host.started
|
|
||||||
report = saved_preflight(host)
|
|
||||||
assert report["stage"] == "before_start"
|
|
||||||
assert report["configuration"][view]["status"] == expected
|
|
||||||
assert_physical_calibration_unverified(report)
|
|
||||||
assert preflight_rows(host)[-1] == report
|
|
||||||
assert clock.positions == positions
|
|
||||||
assert clock.settings == settings
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("state", ["READY", "WAIT_DEVICE", "BASELINE"])
|
|
||||||
def test_progress_distinguishes_configuration_from_unverified_physical_calibration(state):
|
|
||||||
report = CameraPreflight.from_readiness({"side": CameraReadiness(True, True, "")}).as_dict()
|
|
||||||
rendered = render_status_zh({"state": state, "phase": state, "camera_preflight": report})
|
|
||||||
assert "启动前相机配置：side=通过" in rendered
|
|
||||||
assert "启动前实测：光学内参=未验证；相对外参=未验证" in rendered
|
|
||||||
if state in {"READY", "WAIT_DEVICE"}:
|
|
||||||
assert "设备就绪不代表内外参实测通过" in rendered
|
|
||||||
assert "缺少依据时无法判断是否需重新标定" in rendered
|
|
||||||
assert report["intrinsics"]["suggestion"] in rendered
|
|
||||||
assert report["extrinsics"]["suggestion"] in rendered
|
|
||||||
else:
|
|
||||||
assert "检查依据：" not in rendered
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("writer", ["atomic_write_json", "append_jsonl"])
|
|
||||||
def test_failed_start_evidence_write_cannot_start_motion(tmp_path, monkeypatch, writer):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
positions, settings = list(clock.positions), list(clock.settings)
|
|
||||||
|
|
||||||
def failed_write(*args, **kwargs):
|
|
||||||
raise OSError("synthetic_preflight_write_failure")
|
|
||||||
|
|
||||||
monkeypatch.setattr(f"linkerhand_calibration.runtime.coordinator.{writer}", failed_write)
|
|
||||||
with pytest.raises(OSError, match="synthetic_preflight_write_failure"):
|
|
||||||
host.start()
|
|
||||||
assert not host.started
|
|
||||||
assert clock.positions == positions
|
|
||||||
assert clock.settings == settings
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("changed_field", ["k", "d", "r", "p", "width"])
|
|
||||||
def test_real_camera_model_fingerprint_check_is_reported_without_physical_pass(tmp_path, changed_field):
|
|
||||||
message = CameraModelInput(
|
|
||||||
width=640, height=480,
|
|
||||||
k=(600., 0., 300., 0., 620., 220., 0., 0., 1.),
|
|
||||||
d=(.1, .01, 0., 0., 0.),
|
|
||||||
r=tuple(np.eye(3).flat),
|
|
||||||
p=(500., 0., 320., 0., 0., 510., 240., 0., 0., 0., 1., 0.),
|
|
||||||
)
|
|
||||||
identity = CameraCalibrationIdentity("synthetic_camera", message.width, message.height,
|
|
||||||
camera_info_fingerprint(width=message.width, height=message.height,
|
|
||||||
camera_matrix=message.k, distortion=message.d, rectification=message.r, projection=message.p))
|
|
||||||
extrinsics = CameraExtrinsics({"test_view": identity}, "test_view",
|
|
||||||
{"test_view": np.eye(4)}, {})
|
|
||||||
observations = CameraObservations(extrinsics, tmp_path / "raw.jsonl", lambda: 10.)
|
|
||||||
pauses = []
|
|
||||||
|
|
||||||
def accept_and_report(model):
|
|
||||||
observations.accept("test_view", model, started=False, pause=pauses.append, trackers={})
|
|
||||||
return CameraPreflight.from_readiness({"test_view": CameraReadiness(
|
|
||||||
"test_view" in observations.info_received_at, True,
|
|
||||||
observations.errors.get("test_view", ""))}).as_dict()
|
|
||||||
|
|
||||||
valid = accept_and_report(message)
|
|
||||||
assert valid["configuration"]["test_view"]["status"] == "passed"
|
|
||||||
assert_physical_calibration_unverified(valid)
|
|
||||||
old_value = getattr(message, changed_field)
|
|
||||||
new_value = old_value + 1 if changed_field == "width" else (old_value[0] + .01, *old_value[1:])
|
|
||||||
invalid = accept_and_report(replace(message, **{changed_field: new_value}))
|
|
||||||
assert invalid["configuration"]["test_view"]["status"] == "failed"
|
|
||||||
assert_physical_calibration_unverified(invalid)
|
|
||||||
assert not pauses # Before Start this is a device gate, not a running-session pause.
|
|
||||||
assert "test_view" not in observations.matrices
|
|
||||||
|
|
||||||
restored = accept_and_report(message)
|
|
||||||
assert restored["configuration"]["test_view"]["status"] == "passed"
|
|
||||||
assert_physical_calibration_unverified(restored)
|
|
||||||
@@ -1,206 +0,0 @@
|
|||||||
"""Device clocks, transport delay and exposure centre are separate quantities."""
|
|
||||||
|
|
||||||
import gzip
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
import statistics
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.camera_timing import ClockLatch, DeviceFrameClock, MvsCameraTiming
|
|
||||||
|
|
||||||
|
|
||||||
T0 = 1_789_476_000_000_000_000
|
|
||||||
|
|
||||||
|
|
||||||
def latch(elapsed_ns, *, width=200_000, host_jump=0):
|
|
||||||
return ClockLatch(10_000_000_000+elapsed_ns//10,
|
|
||||||
T0+elapsed_ns-width//2+host_jump, T0+elapsed_ns+width//2+host_jump)
|
|
||||||
|
|
||||||
|
|
||||||
def initialized():
|
|
||||||
result = DeviceFrameClock(100_000_000)
|
|
||||||
result.update([latch(0),latch(100_000_000)])
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def test_transport_delay_does_not_change_exposure_timestamp():
|
|
||||||
for delivery_ns in (16_000_000, 120_000_000):
|
|
||||||
clock = initialized()
|
|
||||||
stamp = clock.timestamp(latch(200_000_000).device_tick,
|
|
||||||
received_ns=T0+200_000_000+delivery_ns, exposure_us=5000.)
|
|
||||||
assert stamp == T0+202_500_000
|
|
||||||
|
|
||||||
|
|
||||||
def test_counter_unit_mismatch_and_clock_jump_cannot_forge_new_timestamps():
|
|
||||||
wrong = DeviceFrameClock(1_000_000_000)
|
|
||||||
with pytest.raises(ValueError, match='unit mismatch'):
|
|
||||||
wrong.update([latch(0),latch(100_000_000)])
|
|
||||||
clock = initialized()
|
|
||||||
anchor = clock.anchor
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
clock.update([latch(500_000_000,host_jump=10_000_000)])
|
|
||||||
assert clock.anchor == anchor
|
|
||||||
|
|
||||||
|
|
||||||
def test_frame_reset_duplicates_future_and_stale_clocks_are_rejected():
|
|
||||||
clock = initialized()
|
|
||||||
tick = latch(200_000_000).device_tick
|
|
||||||
clock.timestamp(tick,received_ns=T0+220_000_000,exposure_us=5000.)
|
|
||||||
for duplicate in (tick,tick-1):
|
|
||||||
with pytest.raises(ValueError,match='reset or duplicate'):
|
|
||||||
clock.timestamp(duplicate,received_ns=T0+240_000_000,exposure_us=5000.)
|
|
||||||
with pytest.raises(ValueError,match='future'):
|
|
||||||
clock.timestamp(latch(900_000_000).device_tick,received_ns=T0+240_000_000,exposure_us=5000.)
|
|
||||||
with pytest.raises(ValueError,match='fresh clock'):
|
|
||||||
clock.timestamp(latch(2_500_000_000).device_tick,received_ns=T0+2_520_000_000,exposure_us=5000.)
|
|
||||||
|
|
||||||
|
|
||||||
def test_uncertain_control_transaction_does_not_refresh_the_clock():
|
|
||||||
timing = MvsCameraTiming(None,None,lambda:0,exposure_us=5000.)
|
|
||||||
timing.frame_clock = initialized()
|
|
||||||
anchor = timing.frame_clock.anchor
|
|
||||||
timing._update([latch(500_000_000,width=20_000_000)])
|
|
||||||
assert timing.frame_clock.anchor == anchor
|
|
||||||
timing._update([latch(600_000_000)])
|
|
||||||
assert timing.frame_clock.anchor == latch(600_000_000)
|
|
||||||
|
|
||||||
|
|
||||||
def test_rejected_sync_preserves_failed_transaction_and_previous_anchor(tmp_path):
|
|
||||||
path = tmp_path/'clock.jsonl'
|
|
||||||
timing = MvsCameraTiming(None, None, lambda: 0, exposure_us=5000., journal_path=path)
|
|
||||||
timing.frame_clock = initialized()
|
|
||||||
anchor = timing.frame_clock.anchor
|
|
||||||
jumped = latch(500_000_000, host_jump=10_000_000)
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
timing._update([jumped])
|
|
||||||
assert timing.frame_clock.anchor == anchor
|
|
||||||
row = json.loads(path.read_text()) # Evidence is flushed before invalidation.
|
|
||||||
assert row['kind'] == 'camera_clock_sync_rejected'
|
|
||||||
assert ClockLatch(**row['previous_anchor']) == anchor
|
|
||||||
assert [ClockLatch(**x) for x in row['latches']] == [jumped]
|
|
||||||
replay = DeviceFrameClock(row['ticks_per_second'])
|
|
||||||
replay.anchor = ClockLatch(**row['previous_anchor'])
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
replay.update([ClockLatch(**x) for x in row['latches']])
|
|
||||||
timing.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_raw_clock_brackets_wall_clock_without_replacing_it(monkeypatch):
|
|
||||||
wall = iter((T0, T0+100_000))
|
|
||||||
raw = iter((20_000, 130_000))
|
|
||||||
camera = SimpleNamespace(MV_CC_SetCommandValue=lambda name: 0)
|
|
||||||
timing = MvsCameraTiming(camera, None, lambda: next(wall), exposure_us=5000.,
|
|
||||||
raw_clock_ns=lambda: next(raw))
|
|
||||||
monkeypatch.setattr(timing, '_integer', lambda name: 123456)
|
|
||||||
measured = timing._latch()
|
|
||||||
assert measured == ClockLatch(123456, T0, T0+100_000, 20_000, 130_000)
|
|
||||||
assert measured.midpoint_ns == T0+50_000
|
|
||||||
assert measured.half_width_ns == 50_000
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('slew', [-.005, .005])
|
|
||||||
def test_measured_drift_refreshes_before_continuity_budget_is_spent(monkeypatch, slew):
|
|
||||||
from dataclasses import replace
|
|
||||||
def slewed(elapsed_ns):
|
|
||||||
return latch(elapsed_ns, width=100_000, host_jump=round(elapsed_ns*slew))
|
|
||||||
# At the former fixed interval smooth slew fails the unchanged gate.
|
|
||||||
fixed = DeviceFrameClock(100_000_000)
|
|
||||||
fixed.update([slewed(0), slewed(100_000_000)])
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
fixed.update([slewed(600_000_000)])
|
|
||||||
now = [.1]
|
|
||||||
monkeypatch.setattr('linkerhand_calibration.camera_timing.time.monotonic', lambda: now[0])
|
|
||||||
timing = MvsCameraTiming(None, None, lambda: 0, exposure_us=5000.)
|
|
||||||
timing.frame_clock = DeviceFrameClock(100_000_000)
|
|
||||||
timing._update([slewed(0), slewed(100_000_000)])
|
|
||||||
monkeypatch.setattr(timing, '_latch', lambda: slewed(round(now[0]*1e9)))
|
|
||||||
maximum_error = 0
|
|
||||||
for index in range(1, 1801): # 60 seconds at the real camera's 30 Hz.
|
|
||||||
now[0] = .1+index/30
|
|
||||||
timing.refresh_if_due()
|
|
||||||
elapsed = round((now[0]-.016)*1e9)
|
|
||||||
stamp = timing.frame_clock.timestamp(latch(elapsed).device_tick,
|
|
||||||
received_ns=slewed(round(now[0]*1e9)).midpoint_ns, exposure_us=5000.)
|
|
||||||
expected = T0+round((elapsed+2_500_000)*(1+slew))
|
|
||||||
maximum_error = max(maximum_error, abs(stamp-expected))
|
|
||||||
assert maximum_error < 1_000_000
|
|
||||||
assert .02 <= timing.frame_clock.refresh_interval_seconds < .1
|
|
||||||
# A discontinuity is still rejected, including after successful adaptation.
|
|
||||||
anchor = timing.frame_clock.anchor
|
|
||||||
bad = slewed(round((now[0]+.04)*1e9))
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
timing._update([replace(bad, before_ns=bad.before_ns+10_000_000, after_ns=bad.after_ns+10_000_000)])
|
|
||||||
assert timing.frame_clock.anchor == anchor
|
|
||||||
|
|
||||||
|
|
||||||
def test_startup_anchor_age_is_charged_to_refresh_budget(monkeypatch):
|
|
||||||
monkeypatch.setattr('linkerhand_calibration.camera_timing.time.monotonic', lambda: 10.)
|
|
||||||
timing = MvsCameraTiming(None, None, lambda: 0, exposure_us=5000.)
|
|
||||||
timing.frame_clock = DeviceFrameClock(100_000_000)
|
|
||||||
initial = [latch(i*20_000_000, width=100_000 if i == 0 else 200_000,
|
|
||||||
host_jump=i*100_000) for i in range(8)]
|
|
||||||
timing._update(initial)
|
|
||||||
assert timing.frame_clock.anchor == initial[0]
|
|
||||||
assert timing.next_refresh == 10. # Renew before acquiring the next frame.
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('host_jump', [0, 10_000_000, -1_000_000_000])
|
|
||||||
def test_recovery_revalidates_clock_and_preserves_frame_time_order(monkeypatch, host_jump):
|
|
||||||
timing = MvsCameraTiming(None, None, lambda: 0, exposure_us=5000.)
|
|
||||||
timing.frame_clock = initialized()
|
|
||||||
|
|
||||||
def frame(elapsed_ns):
|
|
||||||
tick = latch(elapsed_ns).device_tick
|
|
||||||
return SimpleNamespace(nDevTimeStampHigh=tick >> 32, nDevTimeStampLow=tick & 0xffffffff,
|
|
||||||
fExposureTime=5000., nFrameNum=1, nHostTimeStamp=0)
|
|
||||||
|
|
||||||
previous = timing.timestamp(frame(200_000_000), T0+220_000_000)
|
|
||||||
with pytest.raises(ValueError, match='discontinuity'):
|
|
||||||
timing._update([latch(400_000_000, host_jump=10_000_000)])
|
|
||||||
timing.invalidate('clock mismatch')
|
|
||||||
with pytest.raises(ValueError, match='no synchronized clock'):
|
|
||||||
timing.timestamp(frame(500_000_000), T0+520_000_000)
|
|
||||||
|
|
||||||
recovery_latches = iter(latch(500_000_000+i*20_000_000, host_jump=host_jump) for i in range(8))
|
|
||||||
monkeypatch.setattr(timing, '_integer', lambda name: 100_000_000)
|
|
||||||
monkeypatch.setattr(timing, '_latch', lambda: next(recovery_latches))
|
|
||||||
monkeypatch.setattr('linkerhand_calibration.camera_timing.time.sleep', lambda seconds: None)
|
|
||||||
timing.refresh_if_due()
|
|
||||||
assert timing.frame_clock.anchor is not None
|
|
||||||
assert timing.last_stamp_ns == previous
|
|
||||||
if host_jump < 0:
|
|
||||||
with pytest.raises(ValueError, match='not increasing'):
|
|
||||||
timing.timestamp(frame(700_000_000), T0+720_000_000+host_jump)
|
|
||||||
assert timing.last_stamp_ns == previous
|
|
||||||
else:
|
|
||||||
assert timing.timestamp(frame(700_000_000), T0+720_000_000+host_jump) == T0+702_500_000+host_jump
|
|
||||||
assert timing.timestamp(frame(1_500_000_000), T0+1_520_000_000+host_jump) == T0+1_502_500_000+host_jump
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('serial', ['DB2163742','DB2163749','DB2163739'])
|
|
||||||
def test_real_camera_counter_units_and_exposure_events(serial):
|
|
||||||
fixture = Path(__file__).parent/'fixtures/camera_clock_receipts.json.gz'
|
|
||||||
data = json.loads(gzip.decompress(fixture.read_bytes()))[serial]
|
|
||||||
assert all(all(status == 0 for status in item['statuses']) for item in data['events']['setup'])
|
|
||||||
latches = [ClockLatch(x['nodes']['DeviceTimestamp']['value'],x['before_ns'],x['read_return_ns'])
|
|
||||||
for x in data['latches']]
|
|
||||||
timing = MvsCameraTiming(None,None,lambda:0,exposure_us=5000.)
|
|
||||||
timing.frame_clock = DeviceFrameClock(data['latches'][0]['nodes']['DeviceTimestampIncrement']['value'])
|
|
||||||
timing._update(latches[:5])
|
|
||||||
clock = timing.frame_clock
|
|
||||||
exposure_events = [x['tick'] for x in data['events']['events'] if x['event']=='ExposureStart']
|
|
||||||
delays, updates = [], 5
|
|
||||||
for frame in data['frames']:
|
|
||||||
if frame['host_unix_ns'] <= latches[4].after_ns:
|
|
||||||
continue
|
|
||||||
if updates < len(latches) and latches[updates].after_ns <= frame['host_unix_ns']:
|
|
||||||
timing._update([latches[updates]])
|
|
||||||
updates += 1
|
|
||||||
stamp = clock.timestamp(frame['camera_tick'],received_ns=frame['host_unix_ns'],exposure_us=5000.)
|
|
||||||
delays.append((frame['host_unix_ns']-stamp)/1e6)
|
|
||||||
if exposure_events[0] <= frame['camera_tick'] <= exposure_events[-1]:
|
|
||||||
assert min(abs(tick-frame['camera_tick']) for tick in exposure_events)*10 <= 200
|
|
||||||
assert len(delays)>40
|
|
||||||
assert 12 < statistics.median(delays) < 16
|
|
||||||
@@ -1,87 +0,0 @@
|
|||||||
"""Continuous observation logging must not fsync under the control lock."""
|
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
from threading import Event
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.artifacts import storage
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from runtime_host_fixture import coordinator_fixture
|
|
||||||
from test_runtime_pose_branch_guard import begin_yaw, branch_event
|
|
||||||
|
|
||||||
|
|
||||||
def test_sweep_records_do_not_wait_for_disk_sync_and_pause_persists_them(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
release, entered = Event(), Event()
|
|
||||||
try:
|
|
||||||
begin_yaw(host, clock)
|
|
||||||
stamp = host.ports.clock_ns()
|
|
||||||
rows = [branch_event(stamp, reason="pose_branch_ambiguous"),
|
|
||||||
{"kind": "diagnostic_observation_frame", "view": "top", "image_stamp_ns": stamp}]
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: (rows, None))
|
|
||||||
def slow_sync(_fd):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(3)
|
|
||||||
with monkeypatch.context() as patch:
|
|
||||||
patch.setattr(storage.os, "fsync", slow_sync)
|
|
||||||
with ThreadPoolExecutor(2) as pool:
|
|
||||||
try:
|
|
||||||
pool.submit(host.receive_detections, DetectionInput("top", stamp, ())).result(timeout=1)
|
|
||||||
clock.now += .04
|
|
||||||
pool.submit(host.receive_feedback, host.command_names,
|
|
||||||
host.profile.command.baseline_values, host.ports.clock_ns()).result(timeout=1)
|
|
||||||
assert not entered.is_set()
|
|
||||||
assert host.state_history[-1].stamp_ns == host.ports.clock_ns()
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
assert host.raw_records[0] == storage.load_jsonl(host.raw_path)[0]
|
|
||||||
captured = host.raw_records[1:]
|
|
||||||
assert [row for row in captured if row["kind"] != "image_observation_frame"] == rows
|
|
||||||
assert len([row for row in captured if row["kind"] == "image_observation_frame"]) == 1
|
|
||||||
assert storage.load_jsonl(host.raw_path)[-len(captured):] == captured
|
|
||||||
persisted = []
|
|
||||||
with monkeypatch.context() as patch:
|
|
||||||
patch.setattr(storage.os, "fsync", lambda _fd: persisted.append(storage.load_jsonl(host.raw_path)))
|
|
||||||
host._pause("test_pause")
|
|
||||||
assert persisted and persisted[-1][-1]["kind"] == "paused"
|
|
||||||
assert persisted[-1][-len(captured)-1:-1] == captured
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("boundary", ["fit", "commit", "diagnostic"])
|
|
||||||
def test_sync_failure_blocks_fitting_publication_and_diagnostic_success(tmp_path, monkeypatch, boundary):
|
|
||||||
from linkerhand_calibration.runtime import coordinator
|
|
||||||
host, _ = coordinator_fixture(tmp_path,
|
|
||||||
diagnostic_capture="o6_thumb" if boundary == "diagnostic" else "")
|
|
||||||
def failed(_path):
|
|
||||||
raise OSError("simulated journal sync failure")
|
|
||||||
try:
|
|
||||||
with monkeypatch.context() as patch:
|
|
||||||
patch.setattr(coordinator, "sync_jsonl", failed)
|
|
||||||
if boundary == "diagnostic":
|
|
||||||
host.execution.session.phase = Phase.DIAGNOSTIC_COMPLETE
|
|
||||||
host._finish_diagnostic_capture()
|
|
||||||
assert host.execution.session.phase == Phase.FAILED
|
|
||||||
assert not (host.parameters.session_dir / "diagnostic_capture.json").exists()
|
|
||||||
else:
|
|
||||||
host.execution.session.phase = Phase.FIT if boundary == "fit" else Phase.PUBLISH
|
|
||||||
with pytest.raises(OSError, match="journal sync failure"):
|
|
||||||
(host._finalize if boundary == "fit" else host._authorize_commit)()
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_close_syncs_last_observation_records(tmp_path, monkeypatch):
|
|
||||||
from linkerhand_calibration.runtime import coordinator
|
|
||||||
host, _ = coordinator_fixture(tmp_path)
|
|
||||||
epoch = host._observation_epoch
|
|
||||||
calls = []
|
|
||||||
monkeypatch.setattr(coordinator, "sync_jsonl", lambda path: calls.append((path, host._observation_epoch)))
|
|
||||||
host.close()
|
|
||||||
assert calls == [(host.raw_path, epoch + 1)]
|
|
||||||
@@ -1,111 +0,0 @@
|
|||||||
"""Pose-cache snapshots stay coherent while camera callbacks keep running."""
|
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
from threading import Event
|
|
||||||
|
|
||||||
from test_observation_capture import frame, make_capture
|
|
||||||
|
|
||||||
|
|
||||||
def test_fingerprint_snapshots_dictionary_and_deques_before_computing_means(monkeypatch):
|
|
||||||
profile, lock, capture = make_capture()
|
|
||||||
lock.begin_session()
|
|
||||||
lock.start_locking()
|
|
||||||
for stamp in range(1, 16):
|
|
||||||
capture.consume(frame(profile, stamp, omit=("thumb_dip",)), collect_installations=True)
|
|
||||||
expected = capture.fingerprint_poses()
|
|
||||||
original_count = capture.installation_count("front:thumb_pitch")
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
original_mean = capture._median_pose
|
|
||||||
snapshots = []
|
|
||||||
def mean(poses):
|
|
||||||
snapshots.append(poses)
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
return original_mean(poses)
|
|
||||||
monkeypatch.setattr(capture, "_median_pose", mean)
|
|
||||||
try:
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(capture.fingerprint_poses)
|
|
||||||
assert entered.wait(2)
|
|
||||||
# Add another role and mutate the existing deque while its old
|
|
||||||
# fingerprint's expensive mean is intentionally blocked.
|
|
||||||
for stamp in range(20, 25):
|
|
||||||
capture.consume(frame(profile, stamp), collect_installations=True)
|
|
||||||
assert capture.installation_count("front:thumb_dip") > 0
|
|
||||||
assert capture.installation_count("front:thumb_pitch") > original_count
|
|
||||||
assert capture.locked_reference_views() == {"front"}
|
|
||||||
assert isinstance(snapshots[0], tuple)
|
|
||||||
assert len(snapshots[0]) == original_count
|
|
||||||
release.set()
|
|
||||||
assert future.result(timeout=2) == expected
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
|
|
||||||
|
|
||||||
def test_fixed_pose_mean_never_holds_the_metadata_read_lock(monkeypatch):
|
|
||||||
profile, lock, capture = make_capture()
|
|
||||||
lock.begin_session()
|
|
||||||
lock.start_locking()
|
|
||||||
for stamp in range(1, 14):
|
|
||||||
capture.consume(frame(profile, stamp))
|
|
||||||
assert not capture.locked_reference_views()
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
original_mean = capture._median_pose
|
|
||||||
def mean(poses):
|
|
||||||
assert isinstance(poses, tuple)
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(2)
|
|
||||||
return original_mean(poses)
|
|
||||||
monkeypatch.setattr(capture, "_median_pose", mean)
|
|
||||||
try:
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(capture.consume, frame(profile, 14))
|
|
||||||
assert entered.wait(2)
|
|
||||||
assert capture.fingerprint_poses() == ({}, {})
|
|
||||||
assert capture.installation_count("front:thumb_pitch") == 0
|
|
||||||
assert capture.locked_reference("front") is None
|
|
||||||
release.set()
|
|
||||||
future.result(timeout=2)
|
|
||||||
assert capture.locked_reference_views() == {"front"}
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
|
|
||||||
|
|
||||||
def test_old_shifted_images_cannot_trigger_false_fixed_reference_drift():
|
|
||||||
profile, lock, capture = make_capture()
|
|
||||||
lock.begin_session()
|
|
||||||
lock.start_locking()
|
|
||||||
for stamp in range(100, 115):
|
|
||||||
capture.consume(frame(profile, stamp))
|
|
||||||
before_stamps = capture._latest_stamp_by_view.copy()
|
|
||||||
before_fingerprint = capture.fingerprint_poses()
|
|
||||||
for stamp in range(1, 11):
|
|
||||||
assert capture.consume(frame(profile, stamp, shift=6)) == ((), None)
|
|
||||||
assert capture._latest_stamp_by_view == before_stamps
|
|
||||||
assert capture.fingerprint_poses() == before_fingerprint
|
|
||||||
assert lock.latest_drift_px["front"] == 0.
|
|
||||||
assert capture.trackers["front"].last_candidate_diagnostics_by_role["front_base"]["observation_stamp_ns"] == 114*40_000_000
|
|
||||||
|
|
||||||
|
|
||||||
def test_old_normal_images_cannot_clear_a_real_fixed_reference_drift():
|
|
||||||
profile, lock, capture = make_capture()
|
|
||||||
lock.begin_session()
|
|
||||||
lock.start_locking()
|
|
||||||
for stamp in range(100, 115):
|
|
||||||
capture.consume(frame(profile, stamp))
|
|
||||||
for stamp in range(120, 129):
|
|
||||||
assert capture.consume(frame(profile, stamp, shift=6))[1] is None
|
|
||||||
for stamp in range(1, 11):
|
|
||||||
assert capture.consume(frame(profile, stamp)) == ((), None)
|
|
||||||
movement = capture.consume(frame(profile, 129, shift=6))[1]
|
|
||||||
assert movement is not None
|
|
||||||
assert movement.confirmation_frames == 10
|
|
||||||
assert movement.drift_px == 6.
|
|
||||||
|
|
||||||
|
|
||||||
def test_capture_reset_resets_each_view_timestamp_gate():
|
|
||||||
profile, lock, capture = make_capture()
|
|
||||||
capture.consume(frame(profile, 100))
|
|
||||||
capture.reset()
|
|
||||||
capture.consume(frame(profile, 1))
|
|
||||||
assert capture._latest_stamp_by_view == {"front": 40_000_000}
|
|
||||||
@@ -1,156 +0,0 @@
|
|||||||
"""One partition contract at scheduling, support and journal boundaries."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import (
|
|
||||||
ADAPTIVE, CapturePlan, configure_training, evidence_digest, select_task_training)
|
|
||||||
from linkerhand_calibration.core.domain.sampling import command_nodes
|
|
||||||
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.task_quality import evaluate_task_input_support
|
|
||||||
from linkerhand_calibration.runtime.training import resolve_capture_plan
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def profile():
|
|
||||||
return configure_training(load_bundled_hand_profile("o30_right_18"), ADAPTIVE)
|
|
||||||
|
|
||||||
|
|
||||||
def decision(profile, task, cycles, action):
|
|
||||||
from linkerhand_calibration.core.fitting.training_quality import TRAINING_DECISION_VERSION
|
|
||||||
row = dict(kind="training_decision", version=TRAINING_DECISION_VERSION,
|
|
||||||
task_name=task.key, assessed_cycles=list(cycles), decision=action,
|
|
||||||
input_plan_sha256=CapturePlan.from_profile(profile).sha256, failures=["noisy"] if action == "fail" else [])
|
|
||||||
return {**row, "decision_sha256": evidence_digest(row)}
|
|
||||||
|
|
||||||
|
|
||||||
def header(profile):
|
|
||||||
return dict(kind="session_start", capture_plan=CapturePlan.from_profile(profile).as_dict())
|
|
||||||
|
|
||||||
|
|
||||||
def support_rows(profile, task, cycles):
|
|
||||||
return [dict(kind="joint_sample", task_name=task.key, joint=task.joints[0],
|
|
||||||
view=profile.measurement.measurements[task.joints[0]].view, sample_phase="steady",
|
|
||||||
cycle=cycle, direction=direction, command_u8=value, image_stamp_ns=index * 10 + node + 1)
|
|
||||||
for index, (cycle, direction) in enumerate((c, d) for c in cycles for d in ("increasing", "decreasing"))
|
|
||||||
for node, value in enumerate((0., 127., 255.))]
|
|
||||||
|
|
||||||
|
|
||||||
def test_optional_round_keeps_holdout_identity_grid_and_motion_segments(profile):
|
|
||||||
units = CalibrationEngine(profile).scan_units()
|
|
||||||
for task in profile.motion.tasks:
|
|
||||||
original_grid = command_nodes(profile, task, holdout=True)
|
|
||||||
profile = select_task_training(profile, task.key, (0, 1))
|
|
||||||
assert command_nodes(profile, task, holdout=True) == original_grid
|
|
||||||
shortened = CalibrationEngine(profile).scan_units()
|
|
||||||
assert len(units) == 144 and len(shortened) == 108
|
|
||||||
assert shortened == tuple(unit for unit in units if unit.cycle != 2)
|
|
||||||
assert all(task.holdout == 3 for task in CapturePlan.from_profile(profile).tasks)
|
|
||||||
|
|
||||||
|
|
||||||
def test_secondary_camera_uses_its_primary_joint_partition():
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import task_for_joint, training_cycles
|
|
||||||
profile = load_bundled_hand_profile("g20_right_19")
|
|
||||||
for primary, secondary in profile.measurement.cross_view_sources.items():
|
|
||||||
assert task_for_joint(profile, secondary) == task_for_joint(profile, primary)
|
|
||||||
assert training_cycles(profile, joint=secondary) == (0, 1, 2)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("mode,cycles", [("interleaved", (0, 1, 2, 3)), ("separate", (4, 5))])
|
|
||||||
def test_command_support_uses_the_correct_phase_partition(profile, mode, cycles):
|
|
||||||
profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode=mode))
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
rows = support_rows(profile, task, cycles)
|
|
||||||
assert evaluate_task_input_support(profile, task, rows).passed
|
|
||||||
# Validation endpoints cannot extend training support.
|
|
||||||
reduced = [row for row in rows if row["cycle"] == cycles[-1] or row["command_u8"] < 255.]
|
|
||||||
assert not evaluate_task_input_support(profile, task, reduced).passed
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("action,expected", [("freeze", 3), ("add_training", 2), ("fail", 2)])
|
|
||||||
def test_session_advances_only_after_bounded_training_decision(profile, monkeypatch, action, expected):
|
|
||||||
from linkerhand_calibration.runtime.engine import SweepQuality
|
|
||||||
driver = SessionExecution(profile)
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
round_cycle = 2 if action == "fail" else 1
|
|
||||||
index = max(i for i, u in enumerate(driver.session._units) if u.task_key == task.key and u.cycle == round_cycle)
|
|
||||||
driver.session._unit_index = index
|
|
||||||
driver.session.phase = CalibrationPhase.EVALUATE
|
|
||||||
driver.completed_units.update(u.identity for u in driver.session._units[:index])
|
|
||||||
monkeypatch.setattr("linkerhand_calibration.runtime.execution.evaluate_capture_unit",
|
|
||||||
lambda *a, **k: SweepQuality(True, (), (), {}))
|
|
||||||
driver.evaluate([], training_decision=decision(profile, task, range(round_cycle + 1), action))
|
|
||||||
if action == "fail":
|
|
||||||
assert driver.session.phase == CalibrationPhase.FAILED
|
|
||||||
else:
|
|
||||||
assert driver.session.current_unit.cycle == expected
|
|
||||||
assert driver.session.status().task.cycle_count == (3 if action == "freeze" else 4)
|
|
||||||
if action == "freeze":
|
|
||||||
assert driver.session.status().task.cycle == 3
|
|
||||||
|
|
||||||
|
|
||||||
def test_journal_rejects_future_holdout_and_changed_decision(profile):
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
row = decision(profile, task, (0, 1), "freeze")
|
|
||||||
current = resolve_capture_plan(profile, [header(profile), row])
|
|
||||||
assert current.quality.task_training_cycles[task.key] == (0, 1)
|
|
||||||
with pytest.raises(ValueError, match="holdout_before_training_freeze"):
|
|
||||||
resolve_capture_plan(profile, [header(profile), support_rows(profile, task, (3,))[0], row])
|
|
||||||
with pytest.raises(ValueError, match="training_decision_invalid"):
|
|
||||||
resolve_capture_plan(profile, [header(profile), {**row, "assessed_cycles": [0, 1, 2]}])
|
|
||||||
with pytest.raises(ValueError, match="training_observation_after_freeze"):
|
|
||||||
resolve_capture_plan(profile, [header(profile), row, support_rows(profile, task, (0,))[0]])
|
|
||||||
with pytest.raises(ValueError, match="matching_plan_header"):
|
|
||||||
resolve_capture_plan(profile, [])
|
|
||||||
with pytest.raises(ValueError, match="third_training_without_decision"):
|
|
||||||
resolve_capture_plan(profile, [header(profile), support_rows(profile, task, (2,))[0]])
|
|
||||||
|
|
||||||
|
|
||||||
def test_fixed_and_adaptive_checkpoints_are_isolated(profile):
|
|
||||||
fixed = configure_training(profile, "fixed")
|
|
||||||
engine = CalibrationEngine(profile)
|
|
||||||
from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION
|
|
||||||
from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION
|
|
||||||
row = {**header(profile), "profile_id": profile.key.profile_id,
|
|
||||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
|
||||||
"pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION,
|
|
||||||
"capture_schedule_version": engine.capture_schedule_version,
|
|
||||||
"calibration_scope": None}
|
|
||||||
from linkerhand_calibration.core.urdf.partial_scope import profile_scope
|
|
||||||
row["calibration_scope"] = profile_scope(profile)
|
|
||||||
assert engine.resume_compatible(row)
|
|
||||||
assert not CalibrationEngine(fixed).resume_compatible(row)
|
|
||||||
row["capture_schedule_version"] = CalibrationEngine(fixed).capture_schedule_version
|
|
||||||
assert not engine.resume_compatible(row)
|
|
||||||
|
|
||||||
|
|
||||||
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):
|
|
||||||
from linkerhand_calibration.runtime.joint_resume import JointResume
|
|
||||||
tasks = profile.motion.tasks[:3]
|
|
||||||
for task in tasks:
|
|
||||||
profile = select_task_training(profile, task.key, (0, 1))
|
|
||||||
engine = CalibrationEngine(profile)
|
|
||||||
resume = JointResume(profile)
|
|
||||||
resume.units = {u.identity for u in engine.scan_units() if u.task_key in {t.key for t in tasks}}
|
|
||||||
resume.units.remove(next(u.identity for u in engine.scan_units() if u.task_key == tasks[1].key))
|
|
||||||
resume.restrict_adaptive_prefix(engine)
|
|
||||||
assert {key[0] for key in resume.units} == {tasks[0].key}
|
|
||||||
assert resume.profile.quality.task_training_cycles == {tasks[0].key: (0, 1)}
|
|
||||||
@@ -1,183 +0,0 @@
|
|||||||
"""Old/edited journals cannot silently acquire current configuration hashes."""
|
|
||||||
|
|
||||||
import copy
|
|
||||||
from dataclasses import replace
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.artifacts.replay import validate_capture_provenance, validate_capture_units
|
|
||||||
from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION, CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.scan_quality import observation_streams
|
|
||||||
|
|
||||||
|
|
||||||
def journal():
|
|
||||||
profile = load_bundled_hand_profile("l6_right_8")
|
|
||||||
# This fixture predates an explicit schedule header and models historical
|
|
||||||
# interleaved capture, not today's separate production schedule.
|
|
||||||
profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved"))
|
|
||||||
hashes = {"source_urdf_sha256": "a"*64, "profile_config_sha256": "b"*64}
|
|
||||||
matrices = {view: [[100., 0., 50.], [0., 100., 50.], [0., 0., 1.]] for view in profile.vision.view_names}
|
|
||||||
header = {"kind": "session_start", "profile_id": profile.key.profile_id, "serial_number": "VIRTUAL",
|
|
||||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
|
||||||
"pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION,
|
|
||||||
"curve_input_domain": profile.curve_input_domain, **hashes}
|
|
||||||
reference = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id,
|
|
||||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
|
||||||
"protected_hashes": {**hashes, "intrinsics_sha256": hashlib.sha256(
|
|
||||||
json.dumps(dict(sorted(matrices.items())), sort_keys=True).encode()).hexdigest()},
|
|
||||||
"fixed_corners_by_view": {view: [[0., 0.], [1., 0.], [1., 1.], [0., 1.]] for view in matrices},
|
|
||||||
"fixed_poses": {view: {"rotation_xyzw": [0., 0., 0., 1.], "translation_xyz_m": [0., 0., 1.]}
|
|
||||||
for view in matrices}, "moving_tag_poses": {}}
|
|
||||||
rows = [header, *({"kind": "rectified_camera_model", "view": view, "camera_matrix": matrix,
|
|
||||||
"input_is_rectified": True, "matrix_source": "CameraInfo.P[:3,:3]"} for view, matrix in matrices.items()), reference]
|
|
||||||
return profile, hashes, rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("mutation", ["old_policy", "old_pose_policy", "changed_hash", "missing_reference", "projection", "input_domain"])
|
|
||||||
def test_changed_or_missing_acquisition_provenance_cannot_publish(mutation):
|
|
||||||
profile, hashes, rows = journal()
|
|
||||||
validate_capture_provenance(profile, "VIRTUAL", hashes, rows)
|
|
||||||
if mutation == "old_policy":
|
|
||||||
rows[0]["acquisition_policy_version"] = "unified_engine_v2"
|
|
||||||
elif mutation == "old_pose_policy":
|
|
||||||
rows[0].pop("pose_tracking_policy_version")
|
|
||||||
elif mutation == "changed_hash":
|
|
||||||
rows[0]["source_urdf_sha256"] = "c"*64
|
|
||||||
elif mutation == "missing_reference":
|
|
||||||
rows.pop()
|
|
||||||
elif mutation == "projection":
|
|
||||||
rows[1]["camera_matrix"][0][0] = 101.
|
|
||||||
else:
|
|
||||||
rows[0]["curve_input_domain"] = "command_rad"
|
|
||||||
with pytest.raises(ValueError, match="capture_provenance"):
|
|
||||||
validate_capture_provenance(profile, "VIRTUAL", hashes, rows)
|
|
||||||
|
|
||||||
|
|
||||||
def test_historical_capture_can_be_checked_without_mixing_it_into_new_motion():
|
|
||||||
profile, hashes, rows = journal()
|
|
||||||
before = copy.deepcopy(rows)
|
|
||||||
validate_capture_provenance(profile, "VIRTUAL", hashes, rows)
|
|
||||||
assert rows == before
|
|
||||||
assert not CalibrationEngine(profile).resume_compatible(rows[0])
|
|
||||||
current = copy.deepcopy(rows)
|
|
||||||
current[0]["capture_schedule_version"] = CAPTURE_SCHEDULE_VERSION
|
|
||||||
validate_capture_provenance(profile, "VIRTUAL", hashes, current)
|
|
||||||
assert CalibrationEngine(profile).resume_compatible(current[0])
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("schedule", ["unknown_future_schedule", {}, []])
|
|
||||||
def test_unknown_capture_schedule_cannot_publish(schedule):
|
|
||||||
profile, hashes, rows = journal()
|
|
||||||
rows[0]["capture_schedule_version"] = schedule
|
|
||||||
with pytest.raises(ValueError, match="capture_provenance"):
|
|
||||||
validate_capture_provenance(profile, "VIRTUAL", hashes, rows)
|
|
||||||
|
|
||||||
|
|
||||||
def complete_journal():
|
|
||||||
profile, hashes, rows = journal()
|
|
||||||
stamp = 0
|
|
||||||
for unit in CalibrationEngine(profile).scan_units():
|
|
||||||
task = next(task for task in profile.motion.tasks if task.key == unit.task_key)
|
|
||||||
common = {"task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction, "attempt": 1}
|
|
||||||
for field, joint, spec in observation_streams(profile, task):
|
|
||||||
for i in range(129):
|
|
||||||
stamp += 1
|
|
||||||
rows.append({**common, field: joint, "view": spec.view, "image_stamp_ns": stamp,
|
|
||||||
"feedback_u8": unit.start+(unit.end-unit.start)*i/128})
|
|
||||||
from linkerhand_calibration.runtime.steady import steady_targets
|
|
||||||
for index, target in enumerate(steady_targets(profile, unit)):
|
|
||||||
for _ in range(3):
|
|
||||||
stamp += 1
|
|
||||||
rows.append({**common, field: joint, "view": spec.view, "image_stamp_ns": stamp,
|
|
||||||
"sample_phase": "steady", "steady_index": index, "steady_target": target})
|
|
||||||
rows.append({**common, "kind": "scan_unit_complete", "passed": True})
|
|
||||||
return profile, hashes, rows
|
|
||||||
|
|
||||||
|
|
||||||
def test_offline_units_use_live_quality_and_require_completed_last_attempt():
|
|
||||||
profile, _, rows = complete_journal()
|
|
||||||
validate_capture_units(profile, rows)
|
|
||||||
failed = copy.deepcopy(rows)
|
|
||||||
failed[-1]["passed"] = False
|
|
||||||
with pytest.raises(ValueError, match="capture_incomplete"):
|
|
||||||
validate_capture_units(profile, failed)
|
|
||||||
failed = rows+[dict(rows[-2], attempt=2)]
|
|
||||||
with pytest.raises(ValueError, match="capture_incomplete"):
|
|
||||||
validate_capture_units(profile, failed)
|
|
||||||
|
|
||||||
|
|
||||||
def test_conflicting_latest_completion_cannot_hide_failed_capture():
|
|
||||||
profile, _, rows = complete_journal()
|
|
||||||
rows.append(dict(rows[-1], passed=False))
|
|
||||||
with pytest.raises(ValueError, match="capture_incomplete"):
|
|
||||||
validate_capture_units(profile, rows)
|
|
||||||
|
|
||||||
|
|
||||||
def release_journal():
|
|
||||||
profile, hashes, rows = complete_journal()
|
|
||||||
# This numerical gate fixture uses the historical interleaved v2 contract.
|
|
||||||
profile = replace(profile, artifacts=replace(profile.artifacts,
|
|
||||||
output_schema_version=2, protected_input_fields=frozenset(hashes)))
|
|
||||||
return profile, hashes, rows
|
|
||||||
|
|
||||||
|
|
||||||
def test_shared_release_gate_checks_post_header_intrinsics():
|
|
||||||
from linkerhand_calibration.runtime.artifacts.capture_validation import validate_capture_for_release
|
|
||||||
profile, hashes, rows = release_journal()
|
|
||||||
reference = next(row for row in rows if row.get("kind") == "fixed_base_reference_locked")
|
|
||||||
hashes.update(intrinsics_sha256=reference["protected_hashes"]["intrinsics_sha256"],
|
|
||||||
raw_samples_sha256="e"*64)
|
|
||||||
validate_capture_for_release(profile, "VIRTUAL", hashes, rows)
|
|
||||||
hashes["intrinsics_sha256"] = "f"*64
|
|
||||||
with pytest.raises(ValueError, match="protected_input_changed:intrinsics_sha256"):
|
|
||||||
validate_capture_for_release(profile, "VIRTUAL", hashes, rows)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("entry", ["online", "offline"])
|
|
||||||
@pytest.mark.parametrize("defect", ["failed_unit", "new_incomplete_attempt", "protected_input"])
|
|
||||||
def test_both_finalization_entries_reject_invalid_capture_before_fitting(tmp_path, monkeypatch, entry, defect):
|
|
||||||
from types import SimpleNamespace
|
|
||||||
from linkerhand_calibration.runtime.artifacts import finalization
|
|
||||||
from linkerhand_calibration.runtime.artifacts.replay import replay_capture
|
|
||||||
from linkerhand_calibration.runtime import runner_support
|
|
||||||
profile, hashes, rows = release_journal()
|
|
||||||
if defect == "failed_unit":
|
|
||||||
rows[-1]["passed"] = False
|
|
||||||
elif defect == "new_incomplete_attempt":
|
|
||||||
rows.append(dict(rows[-2], attempt=2))
|
|
||||||
else:
|
|
||||||
rows[0]["source_urdf_sha256"] = "c"*64
|
|
||||||
monkeypatch.setattr(finalization, "fit_profile_calibration",
|
|
||||||
lambda *args, **kwargs: pytest.fail("invalid capture reached fitting"))
|
|
||||||
directory = tmp_path / "session"
|
|
||||||
expected = "protected_input_changed" if defect == "protected_input" else "capture_incomplete"
|
|
||||||
with pytest.raises(ValueError, match=expected):
|
|
||||||
if entry == "online":
|
|
||||||
finalization.finalize_profile_session(profile=profile, session_dir=directory,
|
|
||||||
serial_number="VIRTUAL", source_urdf=tmp_path/"unused.urdf",
|
|
||||||
protected_inputs=hashes, records=rows, require_motion_evidence=True)
|
|
||||||
else:
|
|
||||||
path = tmp_path / "raw.jsonl"
|
|
||||||
path.write_text("".join(json.dumps(row)+"\n" for row in rows))
|
|
||||||
config = SimpleNamespace(calibration_contract=SimpleNamespace(typed_profile=profile),
|
|
||||||
serial_number="VIRTUAL", source_urdf=tmp_path/"unused.urdf", camera_extrinsics=None)
|
|
||||||
monkeypatch.setattr(runner_support, "protected_inputs", lambda _: dict(hashes))
|
|
||||||
replay_capture(config, path, output=directory, publish=True)
|
|
||||||
diagnostic = json.loads((directory/"capture_validation.json").read_text())
|
|
||||||
assert diagnostic["passed"] is False and diagnostic["publication_allowed"] is False
|
|
||||||
assert expected in diagnostic["reason"]
|
|
||||||
assert not list(directory.glob("*.urdf"))
|
|
||||||
assert not (tmp_path/profile.artifacts.publication_pointer).exists()
|
|
||||||
|
|
||||||
|
|
||||||
def test_stripping_header_does_not_disable_required_production_gate(tmp_path):
|
|
||||||
from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session
|
|
||||||
profile, hashes, rows = release_journal()
|
|
||||||
with pytest.raises(ValueError, match="requires_unique_session"):
|
|
||||||
finalize_profile_session(profile=profile, session_dir=tmp_path, serial_number="VIRTUAL",
|
|
||||||
source_urdf=tmp_path/"unused.urdf", protected_inputs=hashes,
|
|
||||||
records=rows[1:], require_motion_evidence=True)
|
|
||||||
@@ -1,69 +0,0 @@
|
|||||||
"""Live retention cannot change coverage, retries or durable fit evidence."""
|
|
||||||
from copy import deepcopy
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
from linkerhand_calibration.runtime.capture_index import CaptureRecordIndex
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit
|
|
||||||
from linkerhand_calibration.runtime.task_quality import evaluate_task_input_support
|
|
||||||
from linkerhand_calibration.core.fitting.command_sampling import build_sampling_plan
|
|
||||||
from linkerhand_calibration.core.artifacts.storage import append_jsonl
|
|
||||||
from o30_capture_fixture import synthetic_capture
|
|
||||||
|
|
||||||
|
|
||||||
def test_compact_index_preserves_all_twenty_joint_quality_and_sampling_decisions():
|
|
||||||
profile, _, rows, _ = synthetic_capture()
|
|
||||||
index = CaptureRecordIndex(retain_complete=False)
|
|
||||||
index.extend(rows)
|
|
||||||
assert len(index) < len(rows)
|
|
||||||
assert all('parent_pose_common' not in row and 'state_u8' not in row for row in index)
|
|
||||||
full_spans, indexed_spans = {}, {}
|
|
||||||
for unit in CalibrationEngine(profile).scan_units():
|
|
||||||
full = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans=full_spans)
|
|
||||||
compact = evaluate_capture_unit(profile, unit, 1, index, first_cycle_spans=indexed_spans)
|
|
||||||
assert full == compact
|
|
||||||
assert full_spans == indexed_spans
|
|
||||||
for task in profile.motion.tasks:
|
|
||||||
assert evaluate_task_input_support(profile, task, index) == evaluate_task_input_support(profile, task, rows)
|
|
||||||
assert build_sampling_plan(profile, task, index) == build_sampling_plan(profile, task, rows)
|
|
||||||
|
|
||||||
|
|
||||||
def test_failed_attempt_without_joint_samples_still_invalidates_prior_support():
|
|
||||||
from test_task_input_support import capture
|
|
||||||
profile, task, _, rows = capture(held_upper=250.)
|
|
||||||
rows = [dict(row, kind='joint_sample') for row in rows]
|
|
||||||
original = deepcopy(rows)
|
|
||||||
index = CaptureRecordIndex(retain_complete=False)
|
|
||||||
index.extend(rows)
|
|
||||||
assert evaluate_task_input_support(profile, task, index).passed
|
|
||||||
# The new attempt saw only raw images. It cannot fall back to old angles.
|
|
||||||
for direction in ('increasing', 'decreasing'):
|
|
||||||
index.append(dict(kind='pnp_candidate_frame', task_name=task.key, cycle=3,
|
|
||||||
direction=direction, attempt=2, roles={'large': [1]*10000}))
|
|
||||||
assert not evaluate_task_input_support(profile, task, index).passed
|
|
||||||
assert rows == original
|
|
||||||
assert all('roles' not in row for row in index)
|
|
||||||
|
|
||||||
|
|
||||||
def test_journal_backed_coordinator_keeps_full_evidence_only_in_journal(tmp_path):
|
|
||||||
from runtime_host_fixture import coordinator_fixture
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase
|
|
||||||
received = []
|
|
||||||
finalizer = SimpleNamespace(uses_journal=True, started=False, close=lambda: None,
|
|
||||||
start=received.append)
|
|
||||||
host, _ = coordinator_fixture(tmp_path, finalization=finalizer)
|
|
||||||
try:
|
|
||||||
row = dict(kind='pnp_candidate_frame', task_name='example', cycle=0,
|
|
||||||
direction='increasing', roles={'original_corners': [[1.,2.]]*4})
|
|
||||||
append_jsonl(host.raw_path, row)
|
|
||||||
host.capture_index.append(row)
|
|
||||||
assert not host.capture_index.retain_complete
|
|
||||||
assert row in host.raw_records
|
|
||||||
assert all('roles' not in r for r in host.capture_index)
|
|
||||||
host.execution.session.phase = CalibrationPhase.FIT
|
|
||||||
host._finalize()
|
|
||||||
assert received[0].records == ()
|
|
||||||
assert received[0].journal_path == host.raw_path
|
|
||||||
assert received[0].journal_size == host.raw_path.stat().st_size
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,50 +0,0 @@
|
|||||||
from copy import deepcopy
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.chain_candidate_audit import (
|
|
||||||
equivalent_training_signatures, training_solution_families)
|
|
||||||
|
|
||||||
|
|
||||||
def signature():
|
|
||||||
return dict(geometry=np.tile([0., 0., 1., 0., .02, 0.], (3, 1)).tolist(),
|
|
||||||
installation=np.eye(4).tolist(), stage_poses=np.tile(np.eye(4), (3, 1, 1)).tolist())
|
|
||||||
|
|
||||||
|
|
||||||
def test_groups_use_training_state_not_validation_or_cost():
|
|
||||||
state = signature()
|
|
||||||
candidates = [dict(converged=True, training_signature=deepcopy(state),
|
|
||||||
training_cost=i, validation_errors_px=[i*100]) for i in range(64)]
|
|
||||||
assert training_solution_families(candidates) == (tuple(range(64)),)
|
|
||||||
candidates[4]['training_signature']['stage_poses'][0][0][3] = .0001
|
|
||||||
groups = training_solution_families(candidates)
|
|
||||||
assert (4,) in groups and len(groups) == 2
|
|
||||||
candidates[8]['converged'] = False
|
|
||||||
assert (8,) in training_solution_families(candidates)
|
|
||||||
|
|
||||||
|
|
||||||
def test_axis_sign_is_equivalent_but_distinct_installation_is_not():
|
|
||||||
a, b = signature(), signature()
|
|
||||||
b['geometry'] = (np.asarray(b['geometry'])*[-1, -1, -1, 1, 1, 1]).tolist()
|
|
||||||
assert equivalent_training_signatures(a, b)
|
|
||||||
b['installation'] = np.eye(4)
|
|
||||||
b['installation'][:3, :3] = Rotation.from_rotvec([.001, 0, 0]).as_matrix()
|
|
||||||
assert not equivalent_training_signatures(a, b)
|
|
||||||
|
|
||||||
|
|
||||||
def test_complete_link_does_not_merge_a_chain_of_nearby_solutions():
|
|
||||||
rows = []
|
|
||||||
for displacement in (0., 1.5e-6, 3e-6):
|
|
||||||
s = signature()
|
|
||||||
s['installation'][0][3] = displacement
|
|
||||||
rows.append(dict(converged=True, training_signature=s))
|
|
||||||
assert training_solution_families(rows) == ((0, 1), (2,))
|
|
||||||
|
|
||||||
|
|
||||||
def test_invalid_signature_fails_closed():
|
|
||||||
a, b = signature(), signature()
|
|
||||||
b['stage_poses'][0][0][0] = 2.
|
|
||||||
with pytest.raises(ValueError, match='rotation_invalid'):
|
|
||||||
equivalent_training_signatures(a, b)
|
|
||||||
@@ -1,294 +0,0 @@
|
|||||||
"""Deferred acquisition preserves safety and cannot fabricate calibrated zeros."""
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import configure_training, TASKWISE_TWO_ROUND
|
|
||||||
from linkerhand_calibration.profiles.observer_policy import configure_observer_policy
|
|
||||||
from linkerhand_calibration.runtime.chain_capture import DeferredChainCapture
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture
|
|
||||||
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
||||||
from linkerhand_calibration.runtime.execution import SessionExecution
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
|
|
||||||
|
|
||||||
def chain_profile(profile=None):
|
|
||||||
return configure_observer_policy(configure_training(
|
|
||||||
profile or load_bundled_hand_profile('o30_right_18'), TASKWISE_TWO_ROUND))
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('scope', ['one_distal', 'full_hand', 'two_actions'])
|
|
||||||
def test_chain_rejects_auxiliary_actions(scope):
|
|
||||||
from o30_capture_fixture import SOURCE
|
|
||||||
from linkerhand_calibration.core.domain.profile import ObserverPreparationPlan
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.profiles.observer_preparation import configure_observer_preparations, observer_actions
|
|
||||||
profile = configure_observer_preparations(chain_profile(), UrdfKinematicModel(SOURCE))
|
|
||||||
action = observer_actions(profile, 'pinky_dip')[0]
|
|
||||||
if scope != 'full_hand':
|
|
||||||
actions = (action,) if scope == 'one_distal' else (action, action)
|
|
||||||
profile = replace(profile, motion=replace(profile.motion,
|
|
||||||
observer_preparations={'pinky_dip': ObserverPreparationPlan(actions)}))
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_chain_requires_taskwise_feedback_interleaved_plan'):
|
|
||||||
resolve_diagnostic_capture(profile, 'o30_pinky_chain')
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def chain_gate(monkeypatch):
|
|
||||||
profile = chain_profile()
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_pinky_chain')
|
|
||||||
gate = DeferredChainCapture(profile, plan)
|
|
||||||
# These tests isolate raw-window provenance and feedback. The complete
|
|
||||||
# coordinator test exercises real image solvers and the geometric gate.
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import PoseBridgeCheck
|
|
||||||
monkeypatch.setattr(gate, '_frames', lambda *args: ())
|
|
||||||
monkeypatch.setattr('linkerhand_calibration.runtime.chain_reference.check_chain_parent',
|
|
||||||
lambda *args: PoseBridgeCheck('pinky_pip', 'consistent', ''))
|
|
||||||
return gate
|
|
||||||
|
|
||||||
|
|
||||||
def raw_visit(gate, joint, *, version=1, stamp=100, parent=False):
|
|
||||||
spec = gate.profile.motion.joint_zero_references[joint]
|
|
||||||
task = gate.tasks[spec.task_key]
|
|
||||||
motion = MotionCommand('joint_zero', spec.command, 200., task_key=task.key, cycle=0,
|
|
||||||
zero_joints=((task.parent_reference.pose_joint,) if parent else (joint,)),
|
|
||||||
reference_reuse=parent, reference_only=parent)
|
|
||||||
rows = [dict(kind='image_observation_frame', view=gate.view, task_name=task.key,
|
|
||||||
sample_phase='joint_zero', image_stamp_ns=stamp+i, session_epoch=1, motion_version=version,
|
|
||||||
command_vector=list(spec.command), feedback_vector=list(spec.command),
|
|
||||||
tags={role: dict(tag_id=gate.tags[role].tag_id, tag_size_m=gate.tags[role].size_m,
|
|
||||||
corners_xy=[[0., 0.], [20., 0.], [20., 20.], [0., 20.]]) for role in gate.roles})
|
|
||||||
for i in range(gate.profile.acquisition.fixed_reference_minimum_frames)]
|
|
||||||
return motion, rows
|
|
||||||
|
|
||||||
|
|
||||||
def training_return(gate, joint, *, version=1, stamp=150, cycle=0):
|
|
||||||
motion, rows = raw_visit(gate, joint, version=version, stamp=stamp)
|
|
||||||
motion = replace(motion, phase='steady', cycle=cycle, direction='decreasing', zero_joints=())
|
|
||||||
for row in rows:
|
|
||||||
row.update(sample_phase='steady', cycle=cycle, direction='decreasing')
|
|
||||||
return motion, rows
|
|
||||||
|
|
||||||
|
|
||||||
def record_training_return(gate, joint, *, version, stamp):
|
|
||||||
motion, rows = training_return(gate, joint, version=version, stamp=stamp)
|
|
||||||
return gate.steady_visit(motion, rows, epoch=1, version=version)
|
|
||||||
|
|
||||||
|
|
||||||
def test_pending_visits_do_not_supply_geometry_and_parent_checks_both_sources(chain_gate):
|
|
||||||
gate = chain_gate
|
|
||||||
for i, joint in enumerate(gate.plan.chain_joints[:2]):
|
|
||||||
motion, rows = raw_visit(gate, joint, version=i+1, stamp=100*(i+1))
|
|
||||||
result = gate.reference_visit(motion, rows, epoch=1, version=i+1)
|
|
||||||
assert result.attempt.as_record()['no_zero_locked']
|
|
||||||
assert result.record['geometry_status'] == 'pending'
|
|
||||||
record_training_return(gate, joint, version=i+1, stamp=100*(i+1)+50)
|
|
||||||
motion, rows = raw_visit(gate, gate.plan.chain_joints[-1], version=3, stamp=300, parent=True)
|
|
||||||
result = gate.reference_visit(motion, rows, epoch=1, version=3)
|
|
||||||
assert result.attempt is None
|
|
||||||
assert len(result.record['source_images']) == 20
|
|
||||||
assert not result.record['publication_allowed']
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('fault', ['tag', 'duplicate', 'epoch', 'version', 'feedback', 'pixel'])
|
|
||||||
def test_pending_zero_rejects_invalid_windows_without_mutating_state(chain_gate, fault):
|
|
||||||
motion, rows = raw_visit(chain_gate, chain_gate.plan.chain_joints[0])
|
|
||||||
if fault == 'tag':
|
|
||||||
rows[-1]['tags'].pop(chain_gate.roles[-1])
|
|
||||||
elif fault == 'duplicate':
|
|
||||||
rows[-1]['image_stamp_ns'] = rows[-2]['image_stamp_ns']
|
|
||||||
elif fault in ('epoch', 'version'):
|
|
||||||
rows[-1]['session_epoch' if fault == 'epoch' else 'motion_version'] += 1
|
|
||||||
elif fault == 'feedback':
|
|
||||||
rows[-1]['feedback_vector'][10] += 3
|
|
||||||
else:
|
|
||||||
rows[-1]['tags'][chain_gate.roles[0]]['corners_xy'][0][0] += 6
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=1)
|
|
||||||
assert not chain_gate.references
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('delta,accepted', [(7., True), (10., True), (11., False)])
|
|
||||||
def test_parent_feedback_reuses_existing_ten_unit_contract(chain_gate, delta, accepted):
|
|
||||||
for i, joint in enumerate(chain_gate.plan.chain_joints[:2]):
|
|
||||||
motion, rows = raw_visit(chain_gate, joint, version=i+1, stamp=100*(i+1))
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=i+1)
|
|
||||||
record_training_return(chain_gate, joint, version=i+1, stamp=100*(i+1)+50)
|
|
||||||
motion, rows = raw_visit(chain_gate, chain_gate.plan.chain_joints[-1], version=3, stamp=300, parent=True)
|
|
||||||
for row in rows:
|
|
||||||
row['feedback_vector'][5] += delta
|
|
||||||
if accepted:
|
|
||||||
assert chain_gate.reference_visit(motion, rows, epoch=1, version=3).attempt is None
|
|
||||||
else:
|
|
||||||
with pytest.raises(ValueError, match='parent_feedback_changed'):
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=3)
|
|
||||||
|
|
||||||
|
|
||||||
def test_parent_visit_cannot_proceed_without_both_raw_sources(chain_gate):
|
|
||||||
motion, rows = raw_visit(chain_gate, chain_gate.plan.chain_joints[-1], parent=True)
|
|
||||||
with pytest.raises(ValueError, match='parent_visit_missing'):
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=1)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('fault', ['image', 'command'])
|
|
||||||
def test_parent_revisit_rejects_changed_raw_pose_or_command(chain_gate, fault):
|
|
||||||
for i, joint in enumerate(chain_gate.plan.chain_joints[:2]):
|
|
||||||
motion, rows = raw_visit(chain_gate, joint, version=i+1, stamp=100*(i+1))
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=i+1)
|
|
||||||
record_training_return(chain_gate, joint, version=i+1, stamp=100*(i+1)+50)
|
|
||||||
rows[0]['feedback_vector'][0] = 99
|
|
||||||
assert chain_gate.references[1, joint][0]['feedback_vector'][0] != 99
|
|
||||||
motion, rows = raw_visit(chain_gate, chain_gate.plan.chain_joints[-1], version=3, stamp=300, parent=True)
|
|
||||||
for row in rows:
|
|
||||||
if fault == 'image':
|
|
||||||
for corner in row['tags'][chain_gate.roles[0]]['corners_xy']:
|
|
||||||
corner[0] += 6
|
|
||||||
else:
|
|
||||||
row['command_vector'][0] += 1
|
|
||||||
with pytest.raises(ValueError, match='parent_image_changed|current_images_missing'):
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=3)
|
|
||||||
|
|
||||||
|
|
||||||
def test_taskwise_two_plus_one_and_interleaved_holds(chain_gate):
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
gate = chain_gate
|
|
||||||
engine = CalibrationEngine(gate.profile, diagnostic_capture=gate.plan)
|
|
||||||
units = engine.scan_units()
|
|
||||||
assert len(units) == 18
|
|
||||||
assert [(unit.task_key, unit.cycle) for unit in units[::2]] == [
|
|
||||||
(task, cycle) for task in gate.plan.task_keys for cycle in (0, 1, 3)]
|
|
||||||
execution = SessionExecution(gate.profile, diagnostic_capture=gate.plan)
|
|
||||||
execution.session.device_ready()
|
|
||||||
execution.session.start()
|
|
||||||
# Baseline uses the ordinary reviewed whole-hand return path.
|
|
||||||
assert execution.action.phase == Phase.BASELINE
|
|
||||||
effects = execution._make_effects(tuple([128.]*gate.profile.command.command_count))
|
|
||||||
assert effects[-1].target == gate.profile.command.baseline_values
|
|
||||||
execution.session.baseline_complete()
|
|
||||||
execution.session.reference_locked()
|
|
||||||
execution.session.preparation_complete()
|
|
||||||
assert execution.action.phase == Phase.SWEEP
|
|
||||||
effects = execution._make_effects(gate.profile.command.baseline_values)
|
|
||||||
assert {effect.phase for effect in effects} == {'sweep', 'steady'}
|
|
||||||
assert all(a.target == b.target for a, b in zip(effects[::2], effects[1::2]))
|
|
||||||
assert list(dict.fromkeys(effect.target[10] for effect in effects)) == sorted(
|
|
||||||
set(effect.target[10] for effect in effects))
|
|
||||||
|
|
||||||
|
|
||||||
def test_first_training_return_is_raw_and_holdout_cannot_replace_it(chain_gate):
|
|
||||||
gate = chain_gate
|
|
||||||
joint = gate.plan.chain_joints[0]
|
|
||||||
motion, rows = raw_visit(gate, joint)
|
|
||||||
gate.reference_visit(motion, rows, epoch=1, version=1)
|
|
||||||
record = record_training_return(gate, joint, version=2, stamp=200)
|
|
||||||
assert record['cycle'] == 0 and record['direction'] == 'decreasing'
|
|
||||||
assert not record['publication_allowed'] and not record['is_accepted_joint_sample']
|
|
||||||
assert record['source_images'][0] == (gate.view, 200)
|
|
||||||
before = deepcopy(gate.references)
|
|
||||||
motion, rows = training_return(gate, joint, version=3, stamp=300, cycle=3)
|
|
||||||
assert gate.steady_visit(motion, rows, epoch=1, version=3) is None
|
|
||||||
assert gate.references == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_parent_requires_completed_training_return_not_only_initial_hold(chain_gate):
|
|
||||||
for i, joint in enumerate(chain_gate.plan.chain_joints[:2]):
|
|
||||||
motion, rows = raw_visit(chain_gate, joint, version=i+1, stamp=100*(i+1))
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=i+1)
|
|
||||||
motion, rows = raw_visit(chain_gate, chain_gate.plan.chain_joints[-1], version=3, stamp=300, parent=True)
|
|
||||||
with pytest.raises(ValueError, match='parent_training_return_missing'):
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=3)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('fault', ['too_few', 'cycle', 'direction', 'feedback', 'tag'])
|
|
||||||
def test_training_return_rejects_invalid_evidence_transactionally(chain_gate, fault):
|
|
||||||
gate = chain_gate
|
|
||||||
joint = gate.plan.chain_joints[0]
|
|
||||||
motion, rows = raw_visit(gate, joint)
|
|
||||||
gate.reference_visit(motion, rows, epoch=1, version=1)
|
|
||||||
before = deepcopy(gate.references)
|
|
||||||
motion, rows = training_return(gate, joint, version=2, stamp=200)
|
|
||||||
if fault == 'too_few':
|
|
||||||
rows.pop()
|
|
||||||
elif fault == 'cycle':
|
|
||||||
rows[-1]['cycle'] = 3
|
|
||||||
elif fault == 'direction':
|
|
||||||
rows[-1]['direction'] = 'increasing'
|
|
||||||
elif fault == 'feedback':
|
|
||||||
rows[-1]['feedback_vector'][10] += 3
|
|
||||||
else:
|
|
||||||
rows[-1]['tags'].pop(gate.roles[-1])
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
gate.steady_visit(motion, rows, epoch=1, version=2)
|
|
||||||
assert gate.references == before and not gate.training_returns
|
|
||||||
|
|
||||||
|
|
||||||
def test_only_deferred_capture_omits_the_unfitted_zero_excursion(chain_gate):
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
for plan in (None, chain_gate.plan):
|
|
||||||
execution = SessionExecution(chain_gate.profile, diagnostic_capture=plan)
|
|
||||||
execution.session._unit_index = next(i for i, unit in enumerate(execution.session._units)
|
|
||||||
if unit.task_key == 'pinky_mcp_pitch_side' and unit.cycle == 0)
|
|
||||||
execution.session.phase = Phase.PREPARE
|
|
||||||
effects = execution._make_effects(chain_gate.profile.command.baseline_values)
|
|
||||||
excursion = any(m.phase == 'zero_approach' and m.target[10] == 255 for m in effects)
|
|
||||||
assert excursion is (plan is None)
|
|
||||||
assert any(m.phase == 'joint_zero' for m in effects)
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_return_source_is_not_replaced_by_later_callbacks(chain_gate):
|
|
||||||
joint = chain_gate.plan.chain_joints[0]
|
|
||||||
motion, rows = raw_visit(chain_gate, joint)
|
|
||||||
chain_gate.reference_visit(motion, rows, epoch=1, version=1)
|
|
||||||
record_training_return(chain_gate, joint, version=2, stamp=200)
|
|
||||||
before = deepcopy(chain_gate.references)
|
|
||||||
assert record_training_return(chain_gate, joint, version=2, stamp=250) is None
|
|
||||||
assert chain_gate.references == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_training_arc_is_bounded_and_never_consumes_holdout(chain_gate):
|
|
||||||
gate = chain_gate
|
|
||||||
row = dict(kind='diagnostic_observation_frame', view='side', task_name='pinky_mcp_pitch_side',
|
|
||||||
cycle=0, direction='decreasing', sample_phase='sweep', session_epoch=1,
|
|
||||||
feedback_vector=list(gate.profile.command.baseline_values),
|
|
||||||
tags={r: {'reprojection_valid_candidates': [object()]} for r in ('side_base', 'pinky_pip')})
|
|
||||||
for index in range(1000):
|
|
||||||
command = list(gate.profile.command.baseline_values)
|
|
||||||
command[10] = 255-index % 256
|
|
||||||
gate.observe(dict(row, command_vector=command, image_stamp_ns=index+1))
|
|
||||||
key = (1, 'pinky_mcp_pitch')
|
|
||||||
assert len(gate.arcs[key]) <= 129
|
|
||||||
before = tuple(r['image_stamp_ns'] for r in gate.arcs[key].values())
|
|
||||||
gate.observe(dict(row, cycle=3, command_vector=command, image_stamp_ns=2000))
|
|
||||||
gate.observe(dict(row, cycle=1, command_vector=command, image_stamp_ns=2001))
|
|
||||||
gate.training_returns.add(key)
|
|
||||||
gate.observe(dict(row, command_vector=command, image_stamp_ns=2002))
|
|
||||||
assert tuple(r['image_stamp_ns'] for r in gate.arcs[key].values()) == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_stale_training_model_cannot_commit_after_clear(chain_gate):
|
|
||||||
from types import SimpleNamespace
|
|
||||||
request = SimpleNamespace(generation=chain_gate.generation)
|
|
||||||
chain_gate._pending = request
|
|
||||||
chain_gate.clear()
|
|
||||||
assert chain_gate.accept(request, object()) is None
|
|
||||||
assert not chain_gate.models and not chain_gate.pending()
|
|
||||||
|
|
||||||
|
|
||||||
def test_unresolved_training_model_cannot_release_return(chain_gate):
|
|
||||||
from linkerhand_calibration.runtime.chain_reference import ChainReferenceResult
|
|
||||||
joint = chain_gate.plan.chain_joints[0]
|
|
||||||
motion, _ = training_return(chain_gate, joint)
|
|
||||||
assert not chain_gate.reference_ready(motion, 1)
|
|
||||||
chain_gate.models[1, joint] = ChainReferenceResult(False, 'image_motion_families_not_distinguishable')
|
|
||||||
with pytest.raises(ValueError, match='reference_model_unresolved'):
|
|
||||||
chain_gate.reference_ready(motion, 1)
|
|
||||||
|
|
||||||
|
|
||||||
def test_parent_pose_requires_a_resolved_model():
|
|
||||||
from linkerhand_calibration.runtime.chain_reference import ChainReferenceResult, check_chain_parent
|
|
||||||
for result in (None, ChainReferenceResult(False, 'unresolved'), ChainReferenceResult(True, '')):
|
|
||||||
with pytest.raises(ValueError, match='parent_model_unavailable'):
|
|
||||||
check_chain_parent(result, ())
|
|
||||||
@@ -1,124 +0,0 @@
|
|||||||
"""Complete raw chain schedule through the real coordinator, using synthetic FK."""
|
|
||||||
import cv2
|
|
||||||
import json
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from test_chain_capture import chain_profile
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture
|
|
||||||
|
|
||||||
|
|
||||||
def chain_frame(host, values):
|
|
||||||
base = np.array([0., 0., .6])
|
|
||||||
camera_rotation = Rotation.from_euler('xy', [.1, -.15])
|
|
||||||
mcp, pip, dip = [Rotation.from_rotvec([0., 0., values[i]/255*1.3]) for i in (10, 14, 19)]
|
|
||||||
parent = mcp*pip
|
|
||||||
pip_point = mcp.apply([0., -.04, 0.])
|
|
||||||
dip_point = pip_point+parent.apply([0., -.04, 0.])
|
|
||||||
poses = {'side_base': (Rotation.identity(), np.array([-.05, 0., 0.])),
|
|
||||||
'pinky_pip': (parent, pip_point+parent.apply([0., -.01, 0.])),
|
|
||||||
'pinky_dip': (parent*dip, dip_point+(parent*dip).apply([0., -.01, 0.]))}
|
|
||||||
detections = []
|
|
||||||
for tag in next(view.tags for view in host.profile.vision.views if view.name == 'side'):
|
|
||||||
if tag.role not in poses:
|
|
||||||
continue
|
|
||||||
rotation, translation = poses[tag.role]
|
|
||||||
local = rotation.apply(square_object_points(tag.size_m))+translation
|
|
||||||
points = camera_rotation.apply(local)+base
|
|
||||||
pixels = points@host.cameras.matrices['side'].T
|
|
||||||
detections.append(TagDetection(tag.tag_id,
|
|
||||||
tuple(map(tuple, pixels[:, :2]/pixels[:, 2:])), 0, 100.))
|
|
||||||
return DetectionInput('side', host.ports.clock_ns(), tuple(detections))
|
|
||||||
|
|
||||||
|
|
||||||
def test_complete_chain_collects_taskwise_2_plus_1_without_fabricated_zeros(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model='o30',
|
|
||||||
diagnostic_capture='o30_pinky_chain', profile_transform=chain_profile)
|
|
||||||
ready(host, clock)
|
|
||||||
host.cameras.matrices['side'] = np.array([[1400., 0., 320.], [0., 1400., 240.], [0., 0., 1.]])
|
|
||||||
monkeypatch.setattr(host.branch_initialization, 'request', lambda *a, **kw:
|
|
||||||
pytest.fail('batch acquisition must not select an upstream model'))
|
|
||||||
monkeypatch.setattr(host.finalization, 'start', lambda *a, **kw:
|
|
||||||
pytest.fail('pending chain must not publish'))
|
|
||||||
assert host.start().success
|
|
||||||
try:
|
|
||||||
for _ in range(18000):
|
|
||||||
if host.execution.session.phase in (Phase.DIAGNOSTIC_COMPLETE, Phase.PAUSED, Phase.FAILED):
|
|
||||||
break
|
|
||||||
clock.now += .05
|
|
||||||
values = clock.positions[-1] if clock.positions else host.profile.command.baseline_values
|
|
||||||
clock.health_receiver(json.dumps(dict(hand_type='right', model='O30', side='RIGHT',
|
|
||||||
uid='OFFLINE_DEVICE', joint_names=list(host.profile.command.names),
|
|
||||||
online=True, position_mode=True, active_faults=[], joint_faults={})))
|
|
||||||
host.receive_feedback(host.profile.command.names, values, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
host.receive_detections(chain_frame(host, values))
|
|
||||||
assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE, host.reason
|
|
||||||
assert len(host.execution.completed_units) == 18
|
|
||||||
assert not host.execution.zero_references
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
assert tuple(clock.positions[-1]) == host.profile.command.baseline_values
|
|
||||||
# Count real command reversals, including preparation and safe return;
|
|
||||||
# counting only scan units previously hid a fourth full excursion.
|
|
||||||
for channel in (10, 14, 19):
|
|
||||||
commands = np.asarray([host.profile.command.baseline_values,
|
|
||||||
*clock.positions], dtype=float)[:, channel]
|
|
||||||
changed = commands[np.r_[True, np.diff(commands) != 0]]
|
|
||||||
turns = changed[np.r_[True, np.diff(np.sign(np.diff(changed))) != 0, True]]
|
|
||||||
assert turns.tolist() == [0., 255., 0., 255., 0., 255., 0.]
|
|
||||||
from linkerhand_calibration.runtime.resume_storage import load_passed_capture
|
|
||||||
rows = list(load_passed_capture(host.raw_path))
|
|
||||||
assert not any(row.get('kind') in ('joint_sample', 'joint_zero_sample') for row in rows)
|
|
||||||
references = [r for r in rows if r.get('kind') == 'deferred_chain_training_return']
|
|
||||||
assert len(references) == 3
|
|
||||||
assert all(r['cycle'] == 0 and r['direction'] == 'decreasing' for r in references)
|
|
||||||
models = [r for r in rows if r.get('kind') == 'deferred_chain_reference_model']
|
|
||||||
assert len(models) == 2 and all(r['result']['resolved'] for r in models)
|
|
||||||
assert all(not r['publication_allowed'] and not r['physical_zero_validated'] for r in models)
|
|
||||||
assert models[0]['result']['quaternion_xyzw'] == models[1]['result']['quaternion_xyzw']
|
|
||||||
visits = [r for r in rows if r.get('kind') == 'deferred_chain_reference_visit' and r['reference_reuse']]
|
|
||||||
assert visits and all(r['parent_model_joint'] == 'pinky_pip'
|
|
||||||
and r['parent_pose_check']['status'] == 'consistent' for r in visits)
|
|
||||||
decisions = [r for r in rows if r.get('kind') == 'observer_preparation_decision'
|
|
||||||
and r.get('action') == 'excite']
|
|
||||||
assert not decisions
|
|
||||||
for task in host.diagnostic_capture.task_keys:
|
|
||||||
assert {row['cycle'] for row in rows if row.get('kind') == 'image_observation_frame'
|
|
||||||
and row.get('sample_phase') == 'steady' and row.get('task_name') == task} == {0, 1, 3}
|
|
||||||
with pytest.raises(ValueError, match='not_for_publication'):
|
|
||||||
reject_diagnostic_capture(rows)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_chain_missing_distal_tag_cannot_skip_reference_timeout(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, model='o30',
|
|
||||||
diagnostic_capture='o30_pinky_chain', profile_transform=chain_profile)
|
|
||||||
ready(host, clock)
|
|
||||||
host.cameras.matrices['side'] = np.array([[1400., 0., 320.], [0., 1400., 240.], [0., 0., 1.]])
|
|
||||||
assert host.start().success
|
|
||||||
try:
|
|
||||||
for _ in range(2200):
|
|
||||||
if host.execution.session.phase in (Phase.PAUSED, Phase.FAILED):
|
|
||||||
break
|
|
||||||
clock.now += .05
|
|
||||||
values = clock.positions[-1] if clock.positions else host.profile.command.baseline_values
|
|
||||||
clock.health_receiver(json.dumps(dict(hand_type='right', model='O30', side='RIGHT',
|
|
||||||
uid='OFFLINE_DEVICE', joint_names=list(host.profile.command.names),
|
|
||||||
online=True, position_mode=True, active_faults=[], joint_faults={})))
|
|
||||||
host.receive_feedback(host.profile.command.names, values, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
frame = chain_frame(host, values)
|
|
||||||
host.receive_detections(DetectionInput(frame.view, frame.stamp_ns,
|
|
||||||
tuple(tag for tag in frame.detections if tag.tag_id != 5)))
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert host.reason == 'deferred_chain_reference_unavailable:deferred_chain_current_images_missing'
|
|
||||||
assert not host.execution.completed_units and not host.execution.zero_references
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,77 +0,0 @@
|
|||||||
"""Only a declared scope expansion with unchanged preceding measurements can migrate."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
import hashlib
|
|
||||||
from pathlib import Path
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
import yaml
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.urdf.partial_scope import profile_scope
|
|
||||||
from linkerhand_calibration.runtime.checkpoint_extension import ScopeExtension
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.joint_resume import _index_motion_provenance
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def contracts(tmp_path):
|
|
||||||
original = yaml.safe_load((Path(__file__).parents[1] / 'config/profiles/o30_right_18.yaml').read_text())
|
|
||||||
source = deepcopy(original)
|
|
||||||
source['scope']['default_scope'] = 'without_finger_roll'
|
|
||||||
tasks = source['motion']['tasks']
|
|
||||||
tasks.insert(4, tasks.pop())
|
|
||||||
old, new = tmp_path / 'old.yaml', tmp_path / 'new.yaml'
|
|
||||||
old.write_text(yaml.safe_dump(source)); new.write_text(yaml.safe_dump(original))
|
|
||||||
return old, new, original
|
|
||||||
|
|
||||||
|
|
||||||
def test_extension_only_adds_roll_after_existing_sixteen_tasks(contracts):
|
|
||||||
old, new, _ = contracts
|
|
||||||
extension = ScopeExtension.load(old, new)
|
|
||||||
assert len(extension.source.motion.tasks) == 16
|
|
||||||
assert len(extension.target.motion.tasks) == 17
|
|
||||||
assert extension.target.motion.tasks[-1].key == 'fingers_mcp_roll_front'
|
|
||||||
assert extension.target.retained_joints == frozenset()
|
|
||||||
header = {'calibration_scope': profile_scope(extension.source)}
|
|
||||||
assert not CalibrationEngine(extension.target).resume_compatible(header)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('change', ['quality', 'speed', 'order', 'geometry'])
|
|
||||||
def test_extension_cannot_hide_other_contract_changes(contracts, change):
|
|
||||||
old, new, payload = contracts
|
|
||||||
if change == 'quality':
|
|
||||||
payload['acquisition']['minimum_valid_samples'] = 41
|
|
||||||
elif change == 'speed':
|
|
||||||
payload['motion']['tasks'][0]['formal_speed_u8'] -= 1
|
|
||||||
elif change == 'order':
|
|
||||||
tasks = payload['motion']['tasks']; tasks[0], tasks[1] = tasks[1], tasks[0]
|
|
||||||
else:
|
|
||||||
payload['zero']['spatial']['orientation_anchor_joint'] = 'thumb_mcp'
|
|
||||||
new.write_text(yaml.safe_dump(payload))
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
ScopeExtension.load(old, new)
|
|
||||||
|
|
||||||
|
|
||||||
def test_metadata_binds_both_profiles_and_preserves_intrinsics_and_audit(contracts):
|
|
||||||
old, new, _ = contracts
|
|
||||||
extension = ScopeExtension.load(old, new)
|
|
||||||
old_hash, new_hash = (hashlib.sha256(p.read_bytes()).hexdigest() for p in (old, new))
|
|
||||||
inputs = {'profile_config_sha256': new_hash, 'source_urdf_sha256': 'a' * 64}
|
|
||||||
header = {'kind': 'session_start', **inputs, 'profile_config_sha256': old_hash,
|
|
||||||
'calibration_scope': profile_scope(extension.source)}
|
|
||||||
reference = {'kind': 'fixed_base_reference_locked', 'protected_hashes': {
|
|
||||||
**header, 'intrinsics_sha256': 'b' * 64}}
|
|
||||||
checkpoint = SimpleNamespace(header=header, fixed_reference=reference)
|
|
||||||
before = deepcopy((header, reference))
|
|
||||||
migrated, fixed, audit = extension.metadata(checkpoint, inputs, 'source.jsonl', 'c' * 64)
|
|
||||||
assert migrated['profile_config_sha256'] == new_hash
|
|
||||||
assert migrated['calibration_scope'] is None
|
|
||||||
assert fixed['protected_hashes']['intrinsics_sha256'] == 'b' * 64
|
|
||||||
assert audit['original_header'] == header and audit['original_fixed_reference'] == reference
|
|
||||||
assert list(_index_motion_provenance([audit]).values()) == [audit]
|
|
||||||
assert (header, reference) == before
|
|
||||||
for key in ('source_urdf_sha256', 'profile_config_sha256'):
|
|
||||||
changed = {**inputs, key: 'd' * 64}
|
|
||||||
with pytest.raises(ValueError, match='hash_invalid|inputs_changed'):
|
|
||||||
extension.metadata(checkpoint, changed, 'source.jsonl', 'c' * 64)
|
|
||||||
@@ -1,98 +0,0 @@
|
|||||||
"""Changing a pending observer cannot relabel accepted geometry or quality."""
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
import hashlib
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import STAGED, configure_training
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.export import dump_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.observer_policy import configure_observer_policy
|
|
||||||
from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_contract
|
|
||||||
from linkerhand_calibration.runtime.checkpoint_observer_upgrade import ObserverPolicyUpgrade, _copy_indexed_evidence
|
|
||||||
from linkerhand_calibration.runtime.joint_resume import _index_motion_provenance
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def migration():
|
|
||||||
old = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), STAGED),
|
|
||||||
'occlusion_aware_witness_v1')
|
|
||||||
preserved = set(old.measurement.measurements) - {'index_dip'}
|
|
||||||
new = configure_observer_policy(old, preserved_joints=preserved)
|
|
||||||
upgrade = ObserverPolicyUpgrade(old, new, dump_hand_profile(old).encode(), dump_hand_profile(new).encode())
|
|
||||||
inputs = dict(profile_config_sha256=hashlib.sha256(upgrade.target_yaml).hexdigest(), source_urdf_sha256='a'*64)
|
|
||||||
old_inputs = {**inputs, 'profile_config_sha256': hashlib.sha256(upgrade.source_yaml).hexdigest()}
|
|
||||||
header = dict(kind='session_start', **old_inputs, preparation_contract=preparation_contract(old))
|
|
||||||
reference = dict(kind='fixed_base_reference_locked', protected_hashes={**old_inputs, 'intrinsics_sha256': 'b'*64})
|
|
||||||
checkpoint = SimpleNamespace(header=header, fixed_reference=reference, rows=(header, reference),
|
|
||||||
joints=SimpleNamespace(references={joint: object() for joint in preserved},
|
|
||||||
units={('index_pip_side', 0, 'increasing', 'sweep', '')}))
|
|
||||||
return upgrade, checkpoint, inputs
|
|
||||||
|
|
||||||
|
|
||||||
def test_only_pending_strategy_changes_with_complete_audit(migration):
|
|
||||||
upgrade, checkpoint, inputs = migration
|
|
||||||
before = deepcopy((checkpoint.header, checkpoint.fixed_reference))
|
|
||||||
header, reference, audit = upgrade.metadata(checkpoint, inputs, 'source.jsonl', 'c'*64)
|
|
||||||
assert header['preparation_contract'] == preparation_contract(upgrade.target)
|
|
||||||
assert header['profile_config_sha256'] == inputs['profile_config_sha256']
|
|
||||||
assert reference['protected_hashes']['intrinsics_sha256'] == 'b'*64
|
|
||||||
assert set(audit['observer_policy_changes']) == {'index_dip'}
|
|
||||||
assert audit['original_header'] == before[0]
|
|
||||||
assert list(_index_motion_provenance([audit]).values()) == [audit]
|
|
||||||
assert (checkpoint.header, checkpoint.fixed_reference) == before
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('fault', ['quality', 'motion', 'accepted_model', 'frozen', 'source_hash', 'target_hash'])
|
|
||||||
def test_upgrade_rejects_contract_or_accepted_evidence_changes(migration, fault):
|
|
||||||
upgrade, checkpoint, inputs = migration
|
|
||||||
if fault == 'quality':
|
|
||||||
upgrade = replace(upgrade, target=replace(upgrade.target,
|
|
||||||
acquisition=replace(upgrade.target.acquisition, minimum_valid_samples=999)))
|
|
||||||
elif fault == 'motion':
|
|
||||||
upgrade = replace(upgrade, target=replace(upgrade.target,
|
|
||||||
motion=replace(upgrade.target.motion, tasks=tuple(reversed(upgrade.target.motion.tasks)))))
|
|
||||||
elif fault == 'accepted_model':
|
|
||||||
checkpoint.rows += (dict(kind='motion_branch_initialization', status='resolved', zero_joints=['index_dip']),)
|
|
||||||
elif fault == 'frozen':
|
|
||||||
checkpoint.rows += (dict(kind='staged_training_decision', decision='freeze'),)
|
|
||||||
elif fault == 'source_hash':
|
|
||||||
checkpoint.fixed_reference['protected_hashes']['source_urdf_sha256'] = 'd'*64
|
|
||||||
else:
|
|
||||||
inputs['profile_config_sha256'] = 'd'*64
|
|
||||||
with pytest.raises(ValueError, match='observer_upgrade_'):
|
|
||||||
upgrade.metadata(checkpoint, inputs, 'source.jsonl', 'c'*64)
|
|
||||||
|
|
||||||
|
|
||||||
def test_active_observation_migration_only_adds_unmeasured_bounded_paths(migration):
|
|
||||||
from linkerhand_calibration.profiles.observer_preparation import configure_observer_preparations
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from o30_capture_fixture import SOURCE
|
|
||||||
upgrade,checkpoint,inputs=migration
|
|
||||||
target=configure_observer_preparations(upgrade.source,UrdfKinematicModel(SOURCE),
|
|
||||||
preserved_joints=checkpoint.joints.references)
|
|
||||||
upgrade=replace(upgrade,target=target,target_yaml=dump_hand_profile(target).encode(),source_urdf=SOURCE)
|
|
||||||
inputs['profile_config_sha256']=hashlib.sha256(upgrade.target_yaml).hexdigest()
|
|
||||||
_,_,audit=upgrade.metadata(checkpoint,inputs,'source.jsonl','c'*64)
|
|
||||||
assert audit['policy']=='unmeasured_observer_upgrade_v2'
|
|
||||||
assert set(audit['observer_preparations'])=={'index_dip'}
|
|
||||||
checkpoint.joints.references['index_dip']=object()
|
|
||||||
with pytest.raises(ValueError,match='changes_existing_contract'):
|
|
||||||
upgrade.metadata(checkpoint,inputs,'source.jsonl','c'*64)
|
|
||||||
|
|
||||||
|
|
||||||
def test_indexed_checkpoint_migration_copies_content_bound_archive(tmp_path):
|
|
||||||
from linkerhand_calibration.core.artifacts.evidence_journal import (
|
|
||||||
initialize_evidence_journal, append_indexed_records, archive_path, manifest_path,
|
|
||||||
)
|
|
||||||
source = tmp_path/'source/raw_samples.jsonl'
|
|
||||||
source.parent.mkdir()
|
|
||||||
initialize_evidence_journal(source, threshold_bytes=128)
|
|
||||||
append_indexed_records(source, ({'kind': 'pnp_candidate_frame', 'view': 'side',
|
|
||||||
'image_stamp_ns': 1, 'pixels': 'x'*2048},), durable=True)
|
|
||||||
target = tmp_path/'target'; target.mkdir()
|
|
||||||
_copy_indexed_evidence(source, target)
|
|
||||||
assert (target/archive_path(source).name).read_bytes() == archive_path(source).read_bytes()
|
|
||||||
assert (target/manifest_path(source).name).read_bytes() == manifest_path(source).read_bytes()
|
|
||||||
@@ -1,66 +0,0 @@
|
|||||||
"""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)
|
|
||||||
@@ -1,71 +0,0 @@
|
|||||||
"""Command truth is independent of encoder endpoint variability."""
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
import math
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.reference import read_joint_zero_references
|
|
||||||
from linkerhand_calibration.core.fitting.command_motion import fit_command_motion
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.runtime.acquisition import accepted_joint_records
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(scope='module')
|
|
||||||
def inputs(o30_capture):
|
|
||||||
profile, source, rows, _ = o30_capture
|
|
||||||
joint = 'thumb_cmc_roll'
|
|
||||||
sweep = accepted_joint_records(profile, rows)
|
|
||||||
steady = accepted_joint_records(profile, rows, sample_phase='steady')
|
|
||||||
refs = read_joint_zero_references(profile, rows)
|
|
||||||
return profile, UrdfKinematicModel(source), {joint:sweep[joint]}, {joint:steady[joint]}, {joint:refs[joint]}
|
|
||||||
|
|
||||||
|
|
||||||
def test_feedback_drift_does_not_change_command_mapping_or_visual_frame(inputs):
|
|
||||||
p, model, sweep, steady, refs = inputs
|
|
||||||
first = fit_command_motion(*inputs)
|
|
||||||
changed = deepcopy(sweep)
|
|
||||||
for rows in changed.values():
|
|
||||||
for row in rows:
|
|
||||||
row['feedback_u8'] = 3.+row['feedback_u8']*(247. if row['cycle'] < 3 else 250.)/255.
|
|
||||||
second = fit_command_motion(p, model, changed, steady, refs)
|
|
||||||
assert first.mappings == second.mappings
|
|
||||||
assert first.coordinate_curves == second.coordinate_curves
|
|
||||||
diagnostic = second.feedback_diagnostics['thumb_cmc_roll']
|
|
||||||
assert diagnostic['outside_training_support'] > 0
|
|
||||||
assert diagnostic['status'] == 'incomplete_or_inaccurate'
|
|
||||||
assert not diagnostic['required_for_release']
|
|
||||||
|
|
||||||
|
|
||||||
def test_command_holdout_error_cannot_hide_behind_good_feedback(inputs):
|
|
||||||
p, model, sweep, steady, refs = inputs
|
|
||||||
bad = deepcopy(steady)
|
|
||||||
# The independent fourth cycle visibly differs by 7 degrees, while its
|
|
||||||
# commands and encoder samples are untouched. No refit can absorb this.
|
|
||||||
fitted = fit_command_motion(*inputs)
|
|
||||||
axis = fitted.observed_curves['thumb_cmc_roll'].circle['axis_xyz']
|
|
||||||
for row in bad['thumb_cmc_roll']:
|
|
||||||
if row['cycle'] == 3:
|
|
||||||
row['relative_quaternion_xyzw'] = (Rotation.from_quat(row['relative_quaternion_xyzw']) *
|
|
||||||
Rotation.from_rotvec([v*math.radians(7) for v in axis])).as_quat().tolist()
|
|
||||||
with pytest.raises(ValueError, match='steady_command_holdout_failed'):
|
|
||||||
fit_command_motion(p, model, sweep, bad, refs)
|
|
||||||
|
|
||||||
|
|
||||||
def test_release_policy_cannot_silently_enable_unsupported_acquisition(inputs):
|
|
||||||
from linkerhand_calibration.core.domain.profile import validate_profile
|
|
||||||
p = inputs[0]
|
|
||||||
with pytest.raises(ValueError, match='interleaved'):
|
|
||||||
validate_profile(replace(p, acquisition=replace(p.acquisition, command_capture_mode='separate')))
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("feedback", [None, 0.])
|
|
||||||
def test_missing_or_stuck_feedback_does_not_change_command_truth(inputs, feedback):
|
|
||||||
profile, model, sweep, steady, references = inputs
|
|
||||||
frozen = fit_command_motion(*inputs)
|
|
||||||
changed = {name: [dict(row, feedback_u8=feedback) for row in rows] for name, rows in sweep.items()}
|
|
||||||
actual = fit_command_motion(profile, model, changed, steady, references)
|
|
||||||
assert actual.mappings == frozen.mappings
|
|
||||||
assert actual.coordinate_curves == frozen.coordinate_curves
|
|
||||||
assert all(value['status'] == 'unavailable' for value in actual.feedback_diagnostics.values())
|
|
||||||
@@ -1,182 +0,0 @@
|
|||||||
"""Public lookup semantics, report integrity and actual compact-file replay."""
|
|
||||||
|
|
||||||
import json
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from test_unified_artifact_pair import dual_fixture
|
|
||||||
from test_frozen_tag_replay import transform
|
|
||||||
from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher
|
|
||||||
from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper
|
|
||||||
from linkerhand_calibration.runtime.artifacts.serializers.compact_v2 import from_report, REPORT_FILENAME
|
|
||||||
|
|
||||||
|
|
||||||
def stage_compact(tmp_path):
|
|
||||||
session, path, urdf, report, validator = dual_fixture(tmp_path)
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report))
|
|
||||||
payload = from_report(report)
|
|
||||||
path.write_text(json.dumps(payload))
|
|
||||||
return session, path, urdf, report, payload, validator
|
|
||||||
|
|
||||||
|
|
||||||
def test_rad_lookup_report_and_manifest_reader(tmp_path):
|
|
||||||
session, path, urdf, report, payload, validator = stage_compact(tmp_path)
|
|
||||||
assert "quality" not in payload and "command_unit" not in payload
|
|
||||||
assert set(payload["joints"]["drive"]) == {"sdk_channel", "input_values", "angle_rad"}
|
|
||||||
mapping = SerializedJointMapping(payload)
|
|
||||||
expected = .2 + .8*.5 - .07
|
|
||||||
for direction in ("", "increasing", "decreasing"):
|
|
||||||
assert mapping.evaluate([1.1], direction, active_joints=["drive"])["drive"] == pytest.approx(expected)
|
|
||||||
with pytest.raises(ValueError, match="command-only"):
|
|
||||||
SerializedJointMapping(payload, input_kind="feedback")
|
|
||||||
release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session,
|
|
||||||
calibration_json=path, corrected_urdf=urdf, validate=validator)
|
|
||||||
manifest = json.loads(release.manifest.read_text())
|
|
||||||
assert manifest["calibration_report"] == REPORT_FILENAME
|
|
||||||
assert len(manifest["calibration_report_sha256"]) == 64
|
|
||||||
mapper = load_unified_mapper(release.manifest)
|
|
||||||
from linkerhand_calibration.calibrated_joint_state_bridge import load_calibrated_command_mapper
|
|
||||||
assert load_calibrated_command_mapper(path).map_positions([1.1]) == pytest.approx(mapper.map_positions([1.1]))
|
|
||||||
assert mapper.map_positions([1.1])[0] == pytest.approx(expected)
|
|
||||||
assert mapper.map_positions([1.2])[0] == pytest.approx(expected+.04)
|
|
||||||
assert mapper.map_positions([1.1])[0] == pytest.approx(expected)
|
|
||||||
feedback = load_unified_mapper(path, input_kind="feedback")
|
|
||||||
assert feedback.map_positions([.5])[0] == pytest.approx(expected)
|
|
||||||
assert mapper.map_positions([1.1])[1] == pytest.approx(payload["joints"]["passive"]["angle_rad"][32])
|
|
||||||
with pytest.raises(ValueError, match="outside"):
|
|
||||||
mapper.map_positions([3])
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report)+" ")
|
|
||||||
with pytest.raises(ValueError, match="SHA256"):
|
|
||||||
load_unified_mapper(path)
|
|
||||||
|
|
||||||
|
|
||||||
def test_byte_indices_are_exactly_sdk_0_to_255_and_passive_is_recursive(tmp_path):
|
|
||||||
_, _, _, report, _ = dual_fixture(tmp_path)
|
|
||||||
report["command_unit"] = "u8"
|
|
||||||
for kind in ("command", "feedback"):
|
|
||||||
row = report["joints"]["drive"][kind+"_to_rad"]
|
|
||||||
row.update(input_domain=kind+"_u8", input_unit="u8", knots=[0, 128, 255],
|
|
||||||
valid_input_range=[0, 255], angle_rad=[1., .5, 0.],
|
|
||||||
increasing_rad=[1.01, .51, .01], decreasing_rad=[.99, .49, -.01])
|
|
||||||
report["joints"]["nested"] = {"urdf_joint": "nested", "passive": True,
|
|
||||||
"mimic": {"joint": "passive", "multiplier": -.5, "offset_rad": .2}}
|
|
||||||
payload = from_report(report)
|
|
||||||
assert all(set(row) == {"sdk_channel", "angle_rad"} for row in payload["joints"].values())
|
|
||||||
values = payload["joints"]["drive"]["angle_rad"]
|
|
||||||
assert len(values) == 256
|
|
||||||
assert [values[i] for i in (0, 128, 255)] == pytest.approx([1, .5, 0])
|
|
||||||
mapping = SerializedJointMapping(payload)
|
|
||||||
for index in range(256):
|
|
||||||
assert mapping.evaluate([index], "", active_joints=["drive"])["drive"] == values[index]
|
|
||||||
with pytest.raises(ValueError, match="outside"):
|
|
||||||
mapping.evaluate([256], "", active_joints=["drive"])
|
|
||||||
passive = np.asarray(payload["joints"]["passive"]["angle_rad"])
|
|
||||||
assert payload["joints"]["nested"]["angle_rad"] == pytest.approx(.2-.5*passive)
|
|
||||||
# Missing endpoint evidence is not manufactured into a 256-entry export.
|
|
||||||
report["joints"]["drive"]["command_to_rad"].update(knots=[1, 128, 255], valid_input_range=[1, 255])
|
|
||||||
with pytest.raises(ValueError, match="measured_full_command_domain"):
|
|
||||||
from_report(report)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("mutation", ["short_byte", "unknown_branch", "bad_channel", "unordered", "nan"])
|
|
||||||
def test_malformed_public_lookup_is_rejected(tmp_path, mutation):
|
|
||||||
_, _, _, _, payload, _ = stage_compact(tmp_path)
|
|
||||||
row = payload["joints"]["drive"]
|
|
||||||
if mutation == "short_byte":
|
|
||||||
payload["input_unit"] = "u8"
|
|
||||||
for entry in payload["joints"].values():
|
|
||||||
entry.pop("input_values")
|
|
||||||
elif mutation == "unknown_branch":
|
|
||||||
row["increasing_rad"] = row["angle_rad"]
|
|
||||||
elif mutation == "bad_channel":
|
|
||||||
row["sdk_channel"] = .5
|
|
||||||
elif mutation == "unordered":
|
|
||||||
row["input_values"][1] = row["input_values"][0]
|
|
||||||
else:
|
|
||||||
row["angle_rad"][1] = float("nan")
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
SerializedJointMapping(payload)
|
|
||||||
|
|
||||||
|
|
||||||
def test_passive_table_must_match_actual_urdf(tmp_path):
|
|
||||||
_, _, urdf, _, payload, _ = stage_compact(tmp_path)
|
|
||||||
payload["joints"]["passive"]["angle_rad"] = [v+.01 for v in payload["joints"]["passive"]["angle_rad"]]
|
|
||||||
with pytest.raises(ValueError, match="mimic"):
|
|
||||||
validate_compact_urdf_tables(payload, UrdfKinematicModel(urdf))
|
|
||||||
|
|
||||||
|
|
||||||
def test_average_table_itself_must_pass_independent_visual_holdout(tmp_path):
|
|
||||||
session, path, urdf, report, _, validator = stage_compact(tmp_path)
|
|
||||||
# Feedback remains perfect. Alter command training curves consistently, so
|
|
||||||
# the report/lookup correspondence check passes but independent replay fails.
|
|
||||||
mapping = report["joints"]["drive"]["command_to_rad"]
|
|
||||||
for key in ("angle_rad", "increasing_rad", "decreasing_rad"):
|
|
||||||
mapping[key] = [v+.06 for v in mapping[key]]
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report))
|
|
||||||
path.write_text(json.dumps(from_report(report)))
|
|
||||||
with pytest.raises(ValueError, match="compact_command_table_holdout_failed"):
|
|
||||||
ArtifactPublisher(tmp_path, "latest").publish(session_directory=session,
|
|
||||||
calibration_json=path, corrected_urdf=urdf, validate=validator)
|
|
||||||
assert not (tmp_path / "latest").exists()
|
|
||||||
|
|
||||||
|
|
||||||
def test_directional_fit_cannot_hide_excessive_backlash_in_one_table(tmp_path):
|
|
||||||
session, path, urdf, report, _, validator = stage_compact(tmp_path)
|
|
||||||
mapping = report["joints"]["drive"]["command_to_rad"]
|
|
||||||
mean = np.asarray(mapping["angle_rad"])
|
|
||||||
mapping["increasing_rad"] = (mean+.04).tolist()
|
|
||||||
mapping["decreasing_rad"] = (mean-.04).tolist()
|
|
||||||
# Independent analytic generator, using the known mounts rather than the
|
|
||||||
# validator's FK or its fitted curve as truth.
|
|
||||||
base = transform((.13, -.22, .19), (.1, .2, .3))
|
|
||||||
mounts = {"a": transform((.3, .2, -.4), (.003, .012, .018)),
|
|
||||||
"b": transform((-.4, .3, .2), (-.013, .009, .016))}
|
|
||||||
observations = []
|
|
||||||
for row in validator.command_observations:
|
|
||||||
x = (row.sdk_values[0]-.1)/2
|
|
||||||
cad = .2+.8*x+(.04 if row.sdk_directions_by_joint["drive"] == "increasing" else -.04)
|
|
||||||
proximal = transform((.2, -.1, .3), (.02, -.01, .03)) @ transform((0, cad, 0))
|
|
||||||
distal = proximal @ transform((-.1, .2, .1), (.01, 0, .04)) @ transform((.6*cad+.04, 0, 0))
|
|
||||||
pose = base @ (proximal if row.role == "a" else distal) @ mounts[row.role]
|
|
||||||
observations.append(replace(row, common_from_tag=tuple(map(tuple, pose))))
|
|
||||||
validator = replace(validator, command_observations=tuple(observations))
|
|
||||||
# Both direction-specific curves pass with frozen mounts and actual files.
|
|
||||||
path.write_text(json.dumps(report))
|
|
||||||
assert validator(path, urdf)["steady_command_tag_holdout"]["a"]["maximum_deg"] < 1e-6
|
|
||||||
# The same evidence must reject a single direction-independent public table.
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report))
|
|
||||||
path.write_text(json.dumps(from_report(report)))
|
|
||||||
with pytest.raises(ValueError, match="compact_command_table_holdout_failed"):
|
|
||||||
ArtifactPublisher(tmp_path, "latest").publish(session_directory=session,
|
|
||||||
calibration_json=path, corrected_urdf=urdf, validate=validator)
|
|
||||||
assert not (tmp_path / "latest").exists()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("reason", ["compact_command_table_holdout_failed", "compact_byte_requires_measured_full_command_domain"])
|
|
||||||
def test_compact_failure_has_an_actionable_chinese_reason(reason):
|
|
||||||
from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh
|
|
||||||
code, message, suggestion = reason_zh({"reason": reason+":drive"}, model_name="VIRTUAL")
|
|
||||||
assert code.startswith("FIT-COMPACT") and message and suggestion
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("phase", ["validation", "commit"])
|
|
||||||
def test_report_is_protected_through_atomic_publication(tmp_path, phase):
|
|
||||||
session, path, urdf, report, _, validator = stage_compact(tmp_path)
|
|
||||||
publisher = ArtifactPublisher(tmp_path, "latest")
|
|
||||||
def mutate(*args):
|
|
||||||
result = validator(*args)
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report)+" ")
|
|
||||||
return result
|
|
||||||
if phase == "validation":
|
|
||||||
with pytest.raises(ValueError, match="report changed"):
|
|
||||||
publisher.prepare(session_directory=session, calibration_json=path, corrected_urdf=urdf, validate=mutate)
|
|
||||||
else:
|
|
||||||
staged = publisher.prepare(session_directory=session, calibration_json=path, corrected_urdf=urdf, validate=validator)
|
|
||||||
(session / REPORT_FILENAME).write_text(json.dumps(report)+" ")
|
|
||||||
with pytest.raises(ValueError, match="report changed"):
|
|
||||||
publisher.commit(staged)
|
|
||||||
assert not (tmp_path / "latest").exists()
|
|
||||||
@@ -1,84 +0,0 @@
|
|||||||
"""Actual middle PIP/MCP images: shared-pose conditioning, never guessed poses."""
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
import gzip
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.preparation import preparation_contract
|
|
||||||
from linkerhand_calibration.runtime.preparation_witness import WitnessInput,build_preparation_witness,CONDITIONED_WITNESS_POLICY
|
|
||||||
from linkerhand_calibration.runtime.shared_tag_reference import bridge_frames
|
|
||||||
from linkerhand_calibration.runtime.branch_initialization import BranchInitializationRequest,solve_initialization,initialization_artifacts
|
|
||||||
from linkerhand_calibration.runtime.motion_provenance import _initializations
|
|
||||||
from linkerhand_calibration.runtime.preparation_witness_evidence import validate_preparation_witnesses,check_witness_report
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion,image_parameters_for_sampling
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(scope='module')
|
|
||||||
def live_data(tmp_path_factory):
|
|
||||||
data=json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_middle_pip_witness.json.gz').read_bytes()))
|
|
||||||
path=tmp_path_factory.mktemp('middle_witness')/'profile.yaml';path.write_text(data['profile_yaml'])
|
|
||||||
profile=load_hand_profile(path)
|
|
||||||
report=data['failure']; witness=report['preparation_witnesses'][0]
|
|
||||||
rows={r['image_stamp_ns']:r for r in data['rows']}
|
|
||||||
source=WitnessInput(witness['joint'],witness['donor'],tuple(rows[int(k)] for k in witness['source_frame_hashes']),
|
|
||||||
tuple(rows[int(k)] for k in witness['endpoint_frame_hashes']),tuple(witness['channels']))
|
|
||||||
bridge,_=build_preparation_witness(profile,source)
|
|
||||||
frames=bridge_frames(tuple(rows[k] for k in report['source_image_stamps']),bridge.relation)
|
|
||||||
request=BranchInitializationRequest(task_name=report['task_name'],view=report['view'],
|
|
||||||
zero_joints=tuple(report['zero_joints']),session_epoch=report['session_epoch'],motion_version=report['motion_version'],
|
|
||||||
generation=1,frames=tuple(f.evidence for f in frames),relations=(bridge.relation,),existing_evidence_ids=(),
|
|
||||||
moving_roles=('middle_pip',),source_frame_hashes=tuple(report['source_frame_hashes'].items()),
|
|
||||||
image_frames=frames,image_geometry=True,witness_sources=(source,),witness_profile=profile,
|
|
||||||
image_parameters=image_parameters_for_sampling(report.get('image_sampling_policy')))
|
|
||||||
result=solve_initialization(request)
|
|
||||||
assert result.resolved,result.reason
|
|
||||||
_,formal=initialization_artifacts(request,result)
|
|
||||||
records=json.loads(json.dumps([*data['rows'],formal]))
|
|
||||||
return profile,source,request,result,records
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.replay
|
|
||||||
def test_real_full_pip_anchor_resolves_middle_without_relaxing_gates(live_data):
|
|
||||||
profile,source,request,result,records=live_data
|
|
||||||
bridge,legacy=build_preparation_witness(profile,source,conditioned=False)
|
|
||||||
old=resolve_image_motion(request.image_frames,request.relations,pose_bridges=(bridge,),parameters=request.image_parameters)
|
|
||||||
assert not old.resolved and old.reason=='image_motion_shared_pose_unresolved'
|
|
||||||
assert legacy['version']=='preparation_witness_v2' and bridge.source_model is None
|
|
||||||
record=records[-1]
|
|
||||||
assert record['image_model_selection_policy']==CONDITIONED_WITNESS_POLICY
|
|
||||||
assert record['preparation_witnesses'][0]['conditioning']=='shared_zero_pose_v2'
|
|
||||||
assert all(h.pose_bridge_checks[0].status=='consistent' for h in result.hypotheses)
|
|
||||||
assert result.model.maximum_reprojection_error_px==1.5
|
|
||||||
assert result.model.reprojection_tie_px==.03
|
|
||||||
assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns)
|
|
||||||
assert preparation_contract(profile)['witness_pose_conditioning']['joints']==['middle_mcp_pitch']
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.replay
|
|
||||||
def test_formal_witness_and_recipient_replay_original_images(live_data):
|
|
||||||
profile,_,_,_,records=live_data
|
|
||||||
evidence=_initializations(records)
|
|
||||||
validate_preparation_witnesses(profile,records,evidence)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('fault',['policy','version','conditioning'])
|
|
||||||
def test_conditioned_record_cannot_be_downgraded(live_data,fault):
|
|
||||||
report=deepcopy(live_data[-1][-1])
|
|
||||||
if fault=='policy':report['image_model_selection_policy']='training_independent_preparation_witness_holm_v1'
|
|
||||||
elif fault=='version':report['preparation_witnesses'][0]['version']='preparation_witness_v2'
|
|
||||||
else:report['preparation_witnesses'][0]['conditioning']='unknown'
|
|
||||||
with pytest.raises(ValueError):check_witness_report(report)
|
|
||||||
|
|
||||||
|
|
||||||
def test_changed_independent_endpoint_still_vetoes(live_data):
|
|
||||||
profile,source,request,_,_=live_data
|
|
||||||
endpoint=deepcopy(source.endpoint_rows)
|
|
||||||
for row in endpoint:
|
|
||||||
row['tags']['middle_pip']['corners_xy']=[[x+10,y+8] for x,y in row['tags']['middle_pip']['corners_xy']]
|
|
||||||
result=solve_initialization(replace(request,witness_sources=(replace(source,endpoint_rows=tuple(endpoint)),)))
|
|
||||||
assert not result.resolved
|
|
||||||
assert result.model is None
|
|
||||||
@@ -1,398 +0,0 @@
|
|||||||
from pathlib import Path
|
|
||||||
from xml.etree import ElementTree
|
|
||||||
|
|
||||||
import yaml
|
|
||||||
|
|
||||||
|
|
||||||
PACKAGE_ROOT = Path(__file__).resolve().parents[1]
|
|
||||||
|
|
||||||
|
|
||||||
def test_package_owns_both_g20_source_urdfs_and_their_meshes() -> None:
|
|
||||||
for side in ("left", "right"):
|
|
||||||
urdf = (
|
|
||||||
PACKAGE_ROOT
|
|
||||||
/ "urdf"
|
|
||||||
/ f"g20_{side}"
|
|
||||||
/ f"linkerhand_g20_{side}.urdf"
|
|
||||||
)
|
|
||||||
assert urdf.is_file()
|
|
||||||
root = ElementTree.parse(urdf).getroot()
|
|
||||||
mesh_paths = {
|
|
||||||
mesh.attrib["filename"] for mesh in root.findall(".//mesh")
|
|
||||||
}
|
|
||||||
assert mesh_paths
|
|
||||||
assert all(not Path(path).is_absolute() for path in mesh_paths)
|
|
||||||
assert all((urdf.parent / path).is_file() for path in mesh_paths)
|
|
||||||
|
|
||||||
|
|
||||||
def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
|
|
||||||
root = ElementTree.parse(
|
|
||||||
PACKAGE_ROOT / "config" / "fastdds_large_images.xml"
|
|
||||||
).getroot()
|
|
||||||
namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"}
|
|
||||||
profiles = root.find("dds:profiles", namespace)
|
|
||||||
assert profiles is not None
|
|
||||||
segment = profiles.find(
|
|
||||||
".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size",
|
|
||||||
namespace,
|
|
||||||
)
|
|
||||||
assert segment is not None
|
|
||||||
assert int(segment.text) >= 64 * 1024 * 1024
|
|
||||||
participant = profiles.find("dds:participant", namespace)
|
|
||||||
assert participant is not None
|
|
||||||
assert participant.attrib["is_default_profile"] == "true"
|
|
||||||
|
|
||||||
|
|
||||||
def test_three_camera_launches_force_fastdds_large_image_transport() -> None:
|
|
||||||
for launch_name in (
|
|
||||||
"three_camera_calibration.launch.py",
|
|
||||||
"three_camera_extrinsics.launch.py",
|
|
||||||
):
|
|
||||||
launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text()
|
|
||||||
assert 'name="RMW_IMPLEMENTATION"' in launch_text
|
|
||||||
assert 'value="rmw_fastrtps_cpp"' in launch_text
|
|
||||||
assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text
|
|
||||||
assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text
|
|
||||||
|
|
||||||
|
|
||||||
def test_front_tag_parameters_match_namespaced_detector() -> None:
|
|
||||||
config = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
|
|
||||||
)
|
|
||||||
|
|
||||||
detector = config["/apriltag/apriltag"]["ros__parameters"]
|
|
||||||
calibration = config["g20_thumb_calibration"]["ros__parameters"]
|
|
||||||
|
|
||||||
assert detector["tag"]["ids"] == calibration["tag_ids"]
|
|
||||||
assert detector["tag"]["frames"] == calibration["tag_frames"]
|
|
||||||
assert detector["tag"]["sizes"] == calibration["tag_sizes_m"]
|
|
||||||
assert detector["tag"]["ids"] == [0, 1, 2, 3]
|
|
||||||
assert detector["qos_profile"] == "sensor_data"
|
|
||||||
assert detector["detector"]["decimate"] == 1.5
|
|
||||||
assert detector["detector"]["refine"] is True
|
|
||||||
assert detector["detector"]["debug"] is False
|
|
||||||
|
|
||||||
|
|
||||||
def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
|
|
||||||
tags = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text()
|
|
||||||
)
|
|
||||||
expected = {
|
|
||||||
"front": [0, 1, 2, 3, 10],
|
|
||||||
"side": [4, 5, 6, 7],
|
|
||||||
"top": [8, 9],
|
|
||||||
}
|
|
||||||
all_ids = set()
|
|
||||||
for view, ids in expected.items():
|
|
||||||
key = f"/g20_calibration/{view}/apriltag/apriltag"
|
|
||||||
parameters = tags[key]["ros__parameters"]
|
|
||||||
assert parameters["tag"]["ids"] == ids
|
|
||||||
assert parameters["size"] == 0.016
|
|
||||||
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
|
|
||||||
assert parameters["qos_profile"] == "sensor_data"
|
|
||||||
assert parameters["detector"]["decimate"] == 1.5
|
|
||||||
all_ids.update(ids)
|
|
||||||
assert all_ids == set(range(11))
|
|
||||||
assert tags[
|
|
||||||
"/g20_calibration/side/apriltag/apriltag"
|
|
||||||
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
|
|
||||||
|
|
||||||
|
|
||||||
def test_right_19_tag_config_matches_the_physical_layout() -> None:
|
|
||||||
tags = yaml.safe_load(
|
|
||||||
(
|
|
||||||
PACKAGE_ROOT
|
|
||||||
/ "config"
|
|
||||||
/ "three_camera_tags_g20_right_19.yaml"
|
|
||||||
).read_text()
|
|
||||||
)
|
|
||||||
expected = {
|
|
||||||
"front": [0, 1, 2, 3, 10, 11, 12, 13],
|
|
||||||
"side": [4, 5, 6, 7, 14, 15, 16, 17, 18],
|
|
||||||
"top": [8, 9],
|
|
||||||
}
|
|
||||||
all_ids: set[int] = set()
|
|
||||||
for view, ids in expected.items():
|
|
||||||
parameters = tags[
|
|
||||||
f"/g20_calibration/{view}/apriltag/apriltag"
|
|
||||||
]["ros__parameters"]
|
|
||||||
assert parameters["tag"]["ids"] == ids
|
|
||||||
expected_sizes = [0.016 for _tag_id in ids]
|
|
||||||
assert parameters["tag"]["sizes"] == expected_sizes
|
|
||||||
all_ids.update(ids)
|
|
||||||
assert all_ids == set(range(19))
|
|
||||||
side_frames = tags[
|
|
||||||
"/g20_calibration/side/apriltag/apriltag"
|
|
||||||
]["ros__parameters"]["tag"]["frames"]
|
|
||||||
assert side_frames == [
|
|
||||||
"side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip",
|
|
||||||
"middle_pip", "middle_dip", "index_pip", "index_dip",
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
|
||||||
config = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
|
|
||||||
)
|
|
||||||
parameters = config["g20_calibration"]["ros__parameters"]
|
|
||||||
|
|
||||||
assert parameters["tag_size_m"] == 0.016
|
|
||||||
assert parameters["tag_size_override_ids"] == [7, 14, 16, 18]
|
|
||||||
assert parameters["tag_size_overrides_m"] == [0.016] * 4
|
|
||||||
|
|
||||||
assert parameters["baseline_command_u8"] == [
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
127,
|
|
||||||
127,
|
|
||||||
127,
|
|
||||||
127,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
]
|
|
||||||
assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd"
|
|
||||||
assert parameters["normal_calibration_speed"] == 15
|
|
||||||
assert parameters["index_roll_calibration_speed"] == 5
|
|
||||||
assert parameters["index_flex_calibration_speed"] == 10
|
|
||||||
assert parameters["adaptive_formal_speed_enabled"] is True
|
|
||||||
assert parameters["adaptive_formal_speed_max_scale"] == 1.5
|
|
||||||
assert parameters["adaptive_formal_speed_minimum_bins"] == 64
|
|
||||||
assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
|
|
||||||
assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
|
|
||||||
assert parameters["speed_setting_settle_seconds"] >= 0.2
|
|
||||||
assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
|
|
||||||
assert parameters["pnp_group_initialization_frames"] == 8
|
|
||||||
assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
|
|
||||||
assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
|
|
||||||
assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03
|
|
||||||
assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
|
|
||||||
assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
|
|
||||||
assert parameters["baseline_hold_seconds"] == 0.5
|
|
||||||
assert parameters["minimum_baseline_hold_frames"] == 10
|
|
||||||
assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
|
|
||||||
assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
|
|
||||||
assert (
|
|
||||||
parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
|
|
||||||
== 0.3
|
|
||||||
)
|
|
||||||
assert parameters["cross_view_roll_diagnostic_finger"] == ""
|
|
||||||
assert parameters["repetitions"] == 3
|
|
||||||
assert parameters["g20_right_19_repetitions"] >= 4
|
|
||||||
assert parameters["validation_enabled"] is False
|
|
||||||
assert parameters["combination_validation_enabled"] is False
|
|
||||||
assert parameters["combination_validation_frames"] >= 10
|
|
||||||
assert parameters["combination_maximum_position_p95_m"] <= 0.003
|
|
||||||
assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
|
|
||||||
assert parameters["minimum_detection_rate"] == 0.95
|
|
||||||
assert parameters["minimum_detection_hz"] == 15.0
|
|
||||||
assert parameters["minimum_state_span_u8"] >= 240.0
|
|
||||||
assert parameters["endpoint_tolerance_u8"] == 2.0
|
|
||||||
assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
|
|
||||||
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
|
||||||
assert parameters["minimum_sweep_bins"] >= 32
|
|
||||||
assert parameters["maximum_bin_gap"] <= 16
|
|
||||||
assert parameters["automatic_sweep_retry_limit"] == 1
|
|
||||||
assert parameters["automatic_fit_retry_limit"] == 0
|
|
||||||
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
|
||||||
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
|
||||||
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
|
|
||||||
assert parameters["automatic_motion_retry_limit"] == 0
|
|
||||||
assert parameters["provisional_warning_ratio"] == 1.25
|
|
||||||
assert parameters["retry_speed_scales"] == [1.0]
|
|
||||||
assert parameters["retry_endpoint_hold_seconds"] == [0.5]
|
|
||||||
assert parameters["position_timeout_seconds"] >= 20.0
|
|
||||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
|
||||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
|
|
||||||
assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001
|
|
||||||
assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004
|
|
||||||
assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5
|
|
||||||
assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
|
|
||||||
assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
|
|
||||||
assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
|
|
||||||
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
|
|
||||||
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
|
||||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
|
|
||||||
assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.0
|
|
||||||
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
|
|
||||||
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
|
|
||||||
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
|
|
||||||
assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
|
|
||||||
assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
|
|
||||||
assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
|
|
||||||
assert parameters["maximum_hysteresis_deg"] <= 2.0
|
|
||||||
assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
|
|
||||||
assert parameters["command_maximum_direction_gap_deg"] <= 2.0
|
|
||||||
assert parameters["maximum_validation_mae_deg"] <= 1.0
|
|
||||||
assert parameters["maximum_validation_p95_deg"] <= 2.0
|
|
||||||
assert parameters["maximum_validation_error_deg"] <= 3.0
|
|
||||||
assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5
|
|
||||||
for view in ("front", "side", "top"):
|
|
||||||
assert parameters[f"{view}_camera_info_topic"].startswith(
|
|
||||||
f"/g20_calibration/{view}/"
|
|
||||||
)
|
|
||||||
assert parameters[f"{view}_detections_topic"] == (
|
|
||||||
f"/g20_calibration/{view}/apriltag/detections"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
|
|
||||||
config = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
|
|
||||||
)
|
|
||||||
parameters = config["g20_thumb_calibration"]["ros__parameters"]
|
|
||||||
|
|
||||||
assert parameters["angle_estimation_mode"] == "trajectory_center_3d"
|
|
||||||
assert parameters["passive_ip_multiplier"] == 1.02
|
|
||||||
assert parameters["pnp_minimum_valid_rate"] == 0.95
|
|
||||||
assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5
|
|
||||||
assert "pnp_reprojection_tie_px" not in parameters # Shared runtime policy.
|
|
||||||
assert parameters["pnp_tracker_reset_seconds"] == 5.0
|
|
||||||
assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0
|
|
||||||
assert parameters["pnp_group_relative_translation_scale_m"] == 0.01
|
|
||||||
assert parameters["pnp_group_reprojection_weight"] == 0.05
|
|
||||||
assert parameters["pnp_trajectory_reprojection_scale_px"] == 0.1
|
|
||||||
assert parameters["pnp_rigid_rotation_scale_deg"] == 5.0
|
|
||||||
assert parameters["pnp_rigid_p95_accepted_drift_deg"] == 8.0
|
|
||||||
assert parameters["pnp_rigid_maximum_accepted_drift_deg"] == 15.0
|
|
||||||
assert (
|
|
||||||
parameters["pnp_rigid_p95_accepted_distance_drift_m"]
|
|
||||||
<= 0.003
|
|
||||||
)
|
|
||||||
assert (
|
|
||||||
parameters["pnp_rigid_maximum_accepted_distance_drift_m"]
|
|
||||||
<= 0.006
|
|
||||||
)
|
|
||||||
assert parameters["pnp_maximum_pose_jump_deg"] <= 35.0
|
|
||||||
assert parameters["trajectory_maximum_plane_rms_m"] <= 0.004
|
|
||||||
assert parameters["trajectory_maximum_radial_rms_m"] <= 0.004
|
|
||||||
assert parameters["trajectory_minimum_radius_m"] >= 0.005
|
|
||||||
assert parameters["trajectory_minimum_arc_deg"] >= 15.0
|
|
||||||
assert (
|
|
||||||
parameters["trajectory_maximum_root_role_disagreement_deg"]
|
|
||||||
<= 5.0
|
|
||||||
)
|
|
||||||
assert parameters["trajectory_maximum_anchor_drift_m"] <= 0.005
|
|
||||||
assert parameters["trajectory_static_translation_outlier_m"] <= 0.005
|
|
||||||
assert (
|
|
||||||
parameters["trajectory_maximum_static_translation_rms_m"]
|
|
||||||
<= 0.002
|
|
||||||
)
|
|
||||||
assert parameters["minimum_edge_pixels"] == 30.0
|
|
||||||
assert parameters["maximum_static_std_deg"] == 3.0
|
|
||||||
assert parameters["minimum_pose_inlier_rate"] == 0.90
|
|
||||||
assert parameters["repetitions"] == 1
|
|
||||||
assert parameters["command_step"] == 8
|
|
||||||
assert parameters["scan_mode"] == "continuous"
|
|
||||||
assert parameters["continuous_motion_mode"] == "endpoint"
|
|
||||||
assert parameters["auto_start_tip"] is True
|
|
||||||
assert parameters["minimum_detection_hz"] == 15.0
|
|
||||||
assert parameters["stable_frames"] == 5
|
|
||||||
assert parameters["capture_frames"] == 8
|
|
||||||
assert parameters["maximum_stable_spread_deg"] == 3.0
|
|
||||||
assert parameters["maximum_stable_translation_spread_m"] <= 0.003
|
|
||||||
assert parameters["validation_command_count"] == 5
|
|
||||||
assert parameters["continuous_minimum_bins"] >= 32
|
|
||||||
assert parameters["continuous_maximum_bin_gap"] <= 16
|
|
||||||
assert parameters["continuous_segment_minimum_seconds"] >= 0.1
|
|
||||||
assert parameters["continuous_segment_timeout_seconds"] >= 5.0
|
|
||||||
assert parameters["continuous_prepare_timeout_seconds"] >= 30.0
|
|
||||||
|
|
||||||
|
|
||||||
def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
|
|
||||||
config = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text()
|
|
||||||
)
|
|
||||||
parameters = config["g20_thumb_cmc_pitch_zero"]["ros__parameters"]
|
|
||||||
|
|
||||||
assert parameters["t0_id"] == 0
|
|
||||||
assert parameters["t3_id"] == 1
|
|
||||||
assert parameters["joint_name"] == "thumb_cmc_pitch"
|
|
||||||
assert parameters["motor_index"] == 0
|
|
||||||
assert parameters["zero_command_u8"] == 255
|
|
||||||
assert parameters["measure_travel"] is False
|
|
||||||
assert parameters["baseline_command_u8"] == [
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
193,
|
|
||||||
148,
|
|
||||||
105,
|
|
||||||
42,
|
|
||||||
245,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
255,
|
|
||||||
]
|
|
||||||
assert parameters["repetitions"] == 3
|
|
||||||
assert parameters["zero_capture_frames"] == 30
|
|
||||||
assert parameters["trajectory_command_u8"] <= 64
|
|
||||||
assert parameters["trajectory_minimum_state_span_u8"] >= 160.0
|
|
||||||
assert parameters["trajectory_minimum_bins"] >= 18
|
|
||||||
assert parameters["trajectory_minimum_arc_deg"] >= 20.0
|
|
||||||
assert parameters["trajectory_maximum_radial_rms_px"] <= 2.0
|
|
||||||
assert parameters["trajectory_maximum_p95_radial_error_px"] <= 3.5
|
|
||||||
assert parameters["minimum_detection_rate"] == 0.95
|
|
||||||
assert parameters["minimum_edge_pixels"] == 40.0
|
|
||||||
assert parameters["maximum_static_position_rms_px"] <= 1.5
|
|
||||||
assert "maximum_axis_alignment_deg" not in parameters
|
|
||||||
assert "maximum_static_std_deg" not in parameters
|
|
||||||
assert "maximum_anchor_drift_deg" not in parameters
|
|
||||||
assert parameters["camera_alignment_enabled"] is True
|
|
||||||
assert parameters["camera_alignment_max_angle_deg"] <= 0.5
|
|
||||||
assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
|
|
||||||
assert parameters["camera_alignment_required_frames"] >= 10
|
|
||||||
assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8
|
|
||||||
|
|
||||||
|
|
||||||
def test_cmc_roll_config_measures_full_endpoint_travel() -> None:
|
|
||||||
config = yaml.safe_load(
|
|
||||||
(PACKAGE_ROOT / "config" / "cmc_roll_zero_travel.yaml").read_text()
|
|
||||||
)
|
|
||||||
parameters = config[
|
|
||||||
"g20_thumb_cmc_roll_calibration"
|
|
||||||
]["ros__parameters"]
|
|
||||||
|
|
||||||
assert parameters["t0_id"] == 0
|
|
||||||
assert parameters["t3_id"] == 1
|
|
||||||
assert parameters["joint_name"] == "thumb_cmc_roll"
|
|
||||||
assert parameters["motor_index"] == 5
|
|
||||||
assert parameters["zero_command_u8"] == 255
|
|
||||||
assert parameters["trajectory_command_u8"] == 0
|
|
||||||
assert parameters["measure_travel"] is True
|
|
||||||
assert parameters["baseline_command_u8"][5] == 255
|
|
||||||
assert parameters["repetitions"] == 3
|
|
||||||
assert parameters["zero_capture_frames"] == 30
|
|
||||||
assert parameters["trajectory_minimum_state_span_u8"] >= 240.0
|
|
||||||
assert parameters["trajectory_minimum_bins"] >= 30
|
|
||||||
assert parameters["minimum_travel_deg"] >= 20.0
|
|
||||||
assert parameters["maximum_travel_difference_deg"] <= 1.0
|
|
||||||
assert parameters["maximum_round_difference_deg"] == 1.0
|
|
||||||
assert "maximum_axis_alignment_deg" not in parameters
|
|
||||||
assert "maximum_static_std_deg" not in parameters
|
|
||||||
assert "maximum_anchor_drift_deg" not in parameters
|
|
||||||
assert parameters["camera_alignment_enabled"] is True
|
|
||||||
assert parameters["camera_alignment_max_angle_deg"] <= 0.5
|
|
||||||
assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
|
|
||||||
assert parameters["camera_alignment_required_frames"] >= 10
|
|
||||||
assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8
|
|
||||||
@@ -1,244 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import copy
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.compat.legacy.thumb_core import (
|
|
||||||
BASELINE_COMMAND,
|
|
||||||
DIRECTION_DECREASING,
|
|
||||||
DIRECTION_INCREASING,
|
|
||||||
PAIR_IP,
|
|
||||||
PAIR_MCP,
|
|
||||||
PAIR_ROOT,
|
|
||||||
PHASE_ROOT,
|
|
||||||
PHASE_TIP,
|
|
||||||
build_command,
|
|
||||||
create_final_payload,
|
|
||||||
fit_calibration_curves,
|
|
||||||
image_plane_tag_quaternion_xyzw,
|
|
||||||
isotonic_nonincreasing,
|
|
||||||
relative_quaternion_xyzw,
|
|
||||||
rotation_inlier_fraction,
|
|
||||||
rotation_rms_rad,
|
|
||||||
scan_targets,
|
|
||||||
validate_final_payload,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _quaternion(base: Rotation, axis: np.ndarray, angle: float) -> list[float]:
|
|
||||||
value = base * Rotation.from_rotvec(axis * angle)
|
|
||||||
return [float(component) for component in value.as_quat()]
|
|
||||||
|
|
||||||
|
|
||||||
def _synthetic_records(
|
|
||||||
repetitions: int = 3,
|
|
||||||
command_step: int = 1,
|
|
||||||
) -> list[dict]:
|
|
||||||
bases = {
|
|
||||||
PAIR_ROOT: Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
|
|
||||||
PAIR_MCP: Rotation.from_euler("xyz", [-0.15, 0.1, 0.25]),
|
|
||||||
PAIR_IP: Rotation.from_euler("xyz", [0.05, 0.2, -0.2]),
|
|
||||||
}
|
|
||||||
axes = {
|
|
||||||
PAIR_ROOT: np.asarray([0.2, 0.9, -0.1], dtype=float),
|
|
||||||
PAIR_MCP: np.asarray([-0.1, 0.3, 0.95], dtype=float),
|
|
||||||
PAIR_IP: np.asarray([0.05, -0.2, 0.98], dtype=float),
|
|
||||||
}
|
|
||||||
axes = {key: value / np.linalg.norm(value) for key, value in axes.items()}
|
|
||||||
records: list[dict] = []
|
|
||||||
for phase in (PHASE_ROOT, PHASE_TIP):
|
|
||||||
for cycle, direction, command in scan_targets(
|
|
||||||
repetitions, command_step
|
|
||||||
):
|
|
||||||
progress = (255 - command) / 255.0
|
|
||||||
branch = (
|
|
||||||
0.008 * np.sin(np.pi * progress)
|
|
||||||
if direction == DIRECTION_INCREASING
|
|
||||||
else 0.0
|
|
||||||
)
|
|
||||||
root = 0.82 * progress + branch if phase == PHASE_ROOT else 0.0
|
|
||||||
mcp = 1.16 * progress + branch if phase == PHASE_TIP else 0.0
|
|
||||||
ip = 1.018 * mcp + 0.001 * np.sin(np.pi * progress)
|
|
||||||
rotations = {
|
|
||||||
PAIR_ROOT: _quaternion(bases[PAIR_ROOT], axes[PAIR_ROOT], root),
|
|
||||||
PAIR_MCP: _quaternion(bases[PAIR_MCP], axes[PAIR_MCP], mcp),
|
|
||||||
PAIR_IP: _quaternion(bases[PAIR_IP], axes[PAIR_IP], ip),
|
|
||||||
}
|
|
||||||
records.append(
|
|
||||||
{
|
|
||||||
"kind": "sample",
|
|
||||||
"phase": phase,
|
|
||||||
"cycle": cycle,
|
|
||||||
"direction": direction,
|
|
||||||
"command_u8": command,
|
|
||||||
"relative_quaternion_xyzw": rotations,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
return records
|
|
||||||
|
|
||||||
|
|
||||||
def test_build_command_changes_only_selected_channel() -> None:
|
|
||||||
result = build_command(15, 37)
|
|
||||||
assert len(result) == 20
|
|
||||||
assert result[15] == 37
|
|
||||||
assert result[:15] == list(BASELINE_COMMAND[:15])
|
|
||||||
assert result[16:] == list(BASELINE_COMMAND[16:])
|
|
||||||
roll_result = build_command(5, 20)
|
|
||||||
assert roll_result[5] == 20
|
|
||||||
assert roll_result[:5] == list(BASELINE_COMMAND[:5])
|
|
||||||
assert roll_result[6:] == list(BASELINE_COMMAND[6:])
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
build_command(6, 10)
|
|
||||||
|
|
||||||
|
|
||||||
def test_three_cycle_scan_has_every_integer_in_both_directions() -> None:
|
|
||||||
targets = scan_targets(3, command_step=1)
|
|
||||||
assert len(targets) == 3 * 2 * 256
|
|
||||||
assert targets[0] == (0, DIRECTION_DECREASING, 255)
|
|
||||||
assert targets[255] == (0, DIRECTION_DECREASING, 0)
|
|
||||||
assert targets[256] == (0, DIRECTION_INCREASING, 0)
|
|
||||||
assert targets[511] == (0, DIRECTION_INCREASING, 255)
|
|
||||||
|
|
||||||
|
|
||||||
def test_quick_scan_has_bounded_sparse_grid_and_endpoints() -> None:
|
|
||||||
targets = scan_targets(1, command_step=8)
|
|
||||||
assert len(targets) == 66
|
|
||||||
assert targets[0] == (0, DIRECTION_DECREASING, 255)
|
|
||||||
assert targets[32] == (0, DIRECTION_DECREASING, 0)
|
|
||||||
assert targets[33] == (0, DIRECTION_INCREASING, 0)
|
|
||||||
assert targets[-1] == (0, DIRECTION_INCREASING, 255)
|
|
||||||
increasing = [
|
|
||||||
command
|
|
||||||
for _, direction, command in targets
|
|
||||||
if direction == DIRECTION_INCREASING
|
|
||||||
]
|
|
||||||
assert max(np.diff(increasing)) == 8
|
|
||||||
|
|
||||||
|
|
||||||
def test_relative_rotation_cancels_camera_orientation() -> None:
|
|
||||||
camera_to_parent = Rotation.from_euler("xyz", [0.4, -0.2, 0.1])
|
|
||||||
parent_to_child = Rotation.from_rotvec([0.1, 0.3, -0.2])
|
|
||||||
camera_to_child = camera_to_parent * parent_to_child
|
|
||||||
actual = Rotation.from_quat(
|
|
||||||
relative_quaternion_xyzw(
|
|
||||||
camera_to_parent.as_quat(), camera_to_child.as_quat()
|
|
||||||
)
|
|
||||||
)
|
|
||||||
assert (parent_to_child.inv() * actual).magnitude() < 1e-10
|
|
||||||
|
|
||||||
|
|
||||||
def test_image_plane_tag_rotation_uses_ordered_opposite_edges() -> None:
|
|
||||||
angle = np.deg2rad(27.0)
|
|
||||||
x_axis_image = np.asarray([np.cos(angle), -np.sin(angle)])
|
|
||||||
y_axis_image = np.asarray([np.sin(angle), np.cos(angle)])
|
|
||||||
corners = np.asarray(
|
|
||||||
[
|
|
||||||
-x_axis_image - y_axis_image,
|
|
||||||
x_axis_image - y_axis_image,
|
|
||||||
1.1 * x_axis_image + y_axis_image,
|
|
||||||
-0.9 * x_axis_image + y_axis_image,
|
|
||||||
]
|
|
||||||
)
|
|
||||||
actual = Rotation.from_quat(image_plane_tag_quaternion_xyzw(corners))
|
|
||||||
expected = Rotation.from_rotvec([0.0, 0.0, angle])
|
|
||||||
assert (expected.inv() * actual).magnitude() < 1e-10
|
|
||||||
|
|
||||||
|
|
||||||
def test_static_rms_rejects_isolated_planar_pnp_flip() -> None:
|
|
||||||
quaternions = [
|
|
||||||
Rotation.from_rotvec([0.0, np.deg2rad(0.1 * np.sin(index)), 0.0]).as_quat()
|
|
||||||
for index in range(149)
|
|
||||||
]
|
|
||||||
quaternions.append(
|
|
||||||
Rotation.from_rotvec([0.0, np.deg2rad(27.0), 0.0]).as_quat()
|
|
||||||
)
|
|
||||||
|
|
||||||
threshold = np.deg2rad(5.0)
|
|
||||||
assert rotation_rms_rad(quaternions) > np.deg2rad(2.0)
|
|
||||||
assert rotation_rms_rad(
|
|
||||||
quaternions, outlier_threshold_rad=threshold
|
|
||||||
) < np.deg2rad(0.5)
|
|
||||||
assert rotation_inlier_fraction(
|
|
||||||
quaternions, outlier_threshold_rad=threshold
|
|
||||||
) == pytest.approx(149 / 150)
|
|
||||||
|
|
||||||
|
|
||||||
def test_isotonic_projection_is_nonincreasing() -> None:
|
|
||||||
projected = isotonic_nonincreasing([3.0, 2.0, 2.4, 1.0, 0.0])
|
|
||||||
assert np.all(np.diff(projected) <= 0.0)
|
|
||||||
assert projected.tolist() == pytest.approx([3.0, 2.2, 2.2, 1.0, 0.0])
|
|
||||||
|
|
||||||
|
|
||||||
def test_fit_produces_complete_runtime_payload() -> None:
|
|
||||||
fit = fit_calibration_curves(_synthetic_records())
|
|
||||||
assert fit.joints["thumb_cmc_pitch"]["decreasing_rad"][0] == pytest.approx(
|
|
||||||
0.82, abs=2e-3
|
|
||||||
)
|
|
||||||
assert fit.joints["thumb_mcp"]["decreasing_rad"][0] == pytest.approx(
|
|
||||||
1.16, abs=2e-3
|
|
||||||
)
|
|
||||||
assert fit.joints["thumb_ip"]["passive"] is True
|
|
||||||
assert fit.ip_coupling["r_squared"] > 0.999
|
|
||||||
for joint in fit.joints.values():
|
|
||||||
assert len(joint["angle_rad"]) == 256
|
|
||||||
assert len(joint["decreasing_rad"]) == 256
|
|
||||||
assert len(joint["increasing_rad"]) == 256
|
|
||||||
assert joint["angle_rad"][255] == 0.0
|
|
||||||
assert joint["decreasing_rad"][255] == 0.0
|
|
||||||
assert joint["increasing_rad"][255] == 0.0
|
|
||||||
|
|
||||||
payload = create_final_payload(
|
|
||||||
serial_number="G20_LEFT_TEST",
|
|
||||||
fit=fit,
|
|
||||||
validation_errors_rad=[0.01, -0.02],
|
|
||||||
passed=True,
|
|
||||||
)
|
|
||||||
assert set(payload) == {
|
|
||||||
"schema_version",
|
|
||||||
"model",
|
|
||||||
"side",
|
|
||||||
"serial_number",
|
|
||||||
"angle_unit",
|
|
||||||
"command_range",
|
|
||||||
"zero_command_u8",
|
|
||||||
"baseline_command_u8",
|
|
||||||
"joints",
|
|
||||||
"ip_coupling",
|
|
||||||
"quality",
|
|
||||||
}
|
|
||||||
assert payload["schema_version"] == 2
|
|
||||||
for joint in payload["joints"].values():
|
|
||||||
assert "angle_rad" in joint
|
|
||||||
assert "decreasing_rad" not in joint
|
|
||||||
assert "increasing_rad" not in joint
|
|
||||||
validate_final_payload(payload)
|
|
||||||
|
|
||||||
invalid = copy.deepcopy(payload)
|
|
||||||
invalid["joints"]["thumb_mcp"]["angle_rad"].pop()
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
validate_final_payload(invalid)
|
|
||||||
|
|
||||||
|
|
||||||
def test_sparse_fit_interpolates_complete_monotonic_runtime_payload() -> None:
|
|
||||||
fit = fit_calibration_curves(
|
|
||||||
_synthetic_records(repetitions=1, command_step=8)
|
|
||||||
)
|
|
||||||
for joint in fit.joints.values():
|
|
||||||
combined = np.asarray(joint["angle_rad"], dtype=float)
|
|
||||||
assert combined.shape == (256,)
|
|
||||||
assert combined[255] == 0.0
|
|
||||||
assert np.all(np.diff(combined) <= 1e-10)
|
|
||||||
for direction in ("decreasing_rad", "increasing_rad"):
|
|
||||||
curve = np.asarray(joint[direction], dtype=float)
|
|
||||||
assert curve.shape == (256,)
|
|
||||||
assert curve[255] == 0.0
|
|
||||||
assert np.all(np.diff(curve) <= 1e-10)
|
|
||||||
assert fit.joints["thumb_cmc_pitch"]["decreasing_rad"][0] == pytest.approx(
|
|
||||||
0.82, abs=2e-3
|
|
||||||
)
|
|
||||||
assert fit.joints["thumb_mcp"]["decreasing_rad"][0] == pytest.approx(
|
|
||||||
1.16, abs=2e-3
|
|
||||||
)
|
|
||||||
@@ -1,25 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.fitting.observed_motion import fit_byte_observed_motion
|
|
||||||
|
|
||||||
|
|
||||||
def test_positive_sdk_direction_reorients_observation_axis_and_table_together():
|
|
||||||
rows = []
|
|
||||||
mount = Rotation.from_euler("xyz", [.23, -.17, .31])
|
|
||||||
for cycle in range(4):
|
|
||||||
for direction in ("increasing", "decreasing"):
|
|
||||||
for index, value in enumerate(np.linspace(0, 255, 86).astype(int)):
|
|
||||||
angle = value/255*.8
|
|
||||||
q = Rotation.from_euler("z", angle) * mount
|
|
||||||
rows.append({"sample_id": f"{cycle}/{direction}/{index}", "cycle": cycle,
|
|
||||||
"direction": direction, "command_u8": int(value),
|
|
||||||
"relative_quaternion_xyzw": q.as_quat().tolist()})
|
|
||||||
curves, errors, evidence = fit_byte_observed_motion({"arbitrary_joint": rows}, source_model=None,
|
|
||||||
mimic_sources={}, baseline_by_joint={"arbitrary_joint": 0}, direction_by_joint={"arbitrary_joint": 1},
|
|
||||||
input_domain="command_u8")
|
|
||||||
curve = curves["arbitrary_joint"]
|
|
||||||
assert curve.angle_rad[-1] > curve.angle_rad[0]
|
|
||||||
assert abs(curve.angle_rad[-1]-.8) < 1e-6
|
|
||||||
assert max(abs(e) for e in errors["arbitrary_joint"]) < 1e-6
|
|
||||||
assert not evidence
|
|
||||||
@@ -1,49 +0,0 @@
|
|||||||
"""Larger witness arcs must be identical to an existing clearance-bound path."""
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.profiles.preparation import configure_preparation, validate_preparation_witnesses
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import configure_training
|
|
||||||
from linkerhand_calibration.core.domain.profile import ModelPreparation
|
|
||||||
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
|
|
||||||
from o30_capture_fixture import SOURCE
|
|
||||||
|
|
||||||
|
|
||||||
def configuration():
|
|
||||||
profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'),
|
|
||||||
'staged_2_to_3'), 'occlusion_aware_witness_v1')
|
|
||||||
ref = profile.motion.joint_zero_references['middle_pip']
|
|
||||||
witness = ModelPreparation('middle_pip', ref.command, (ref.command, ref.approach_commands[0]))
|
|
||||||
return profile, witness
|
|
||||||
|
|
||||||
|
|
||||||
def validate(profile, witness):
|
|
||||||
witnesses = dict(profile.motion.preparation_witnesses, middle_mcp_pitch=witness)
|
|
||||||
candidate = replace(profile, motion=replace(profile.motion, preparation_witnesses=witnesses))
|
|
||||||
validate_preparation_witnesses(candidate, UrdfKinematicModel(SOURCE))
|
|
||||||
|
|
||||||
|
|
||||||
def test_existing_pip_path_can_be_reused_at_identical_clearance():
|
|
||||||
profile, witness = configuration()
|
|
||||||
validate(profile, witness)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('change', ['undeclared_amplitude', 'clearance', 'other_joint', 'roll_full_range'])
|
|
||||||
def test_large_witness_must_match_declared_single_joint_path(change):
|
|
||||||
profile, witness = configuration()
|
|
||||||
excursion = list(witness.approach_commands[1])
|
|
||||||
if change == 'undeclared_amplitude':
|
|
||||||
excursion[12] = 200.0
|
|
||||||
elif change == 'other_joint':
|
|
||||||
excursion[8] = 5.0
|
|
||||||
elif change == 'clearance':
|
|
||||||
command = list(witness.command); command[9] = 0.0; excursion[9] = 0.0
|
|
||||||
witness = replace(witness, command=tuple(command), approach_commands=(tuple(command),tuple(excursion)))
|
|
||||||
else:
|
|
||||||
witness = profile.motion.preparation_witnesses['middle_mcp_pitch']
|
|
||||||
excursion = list(witness.command); excursion[3] = 0.0
|
|
||||||
witness = replace(witness, approach_commands=(witness.command, tuple(excursion)))
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
validate(profile, witness)
|
|
||||||
@@ -1,109 +0,0 @@
|
|||||||
"""Image lookahead cannot monopolize feedback callbacks or grow a backlog."""
|
|
||||||
|
|
||||||
from threading import Event
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.runtime.detection_dispatch import DetectionDispatcher
|
|
||||||
|
|
||||||
|
|
||||||
def observation(stamp, view='front'):
|
|
||||||
return SimpleNamespace(view=view, stamp_ns=stamp)
|
|
||||||
|
|
||||||
|
|
||||||
def test_waits_for_feedback_to_cover_image_before_delivering():
|
|
||||||
delivered = Event()
|
|
||||||
dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(),
|
|
||||||
maximum_wait_seconds=1., failed=lambda error: None)
|
|
||||||
try:
|
|
||||||
dispatcher.submit(observation(20))
|
|
||||||
dispatcher.feedback_available(19)
|
|
||||||
assert not delivered.wait(.02)
|
|
||||||
dispatcher.feedback_available(21)
|
|
||||||
assert delivered.wait(.2)
|
|
||||||
finally:
|
|
||||||
dispatcher.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_slow_image_work_retains_only_latest_pending_frame_per_view():
|
|
||||||
running, release, finished = Event(), Event(), Event()
|
|
||||||
images = []
|
|
||||||
def deliver(row):
|
|
||||||
images.append(row.stamp_ns)
|
|
||||||
if row.stamp_ns == 1:
|
|
||||||
running.set()
|
|
||||||
assert release.wait(1.)
|
|
||||||
else:
|
|
||||||
finished.set()
|
|
||||||
dispatcher = DetectionDispatcher(('front',), deliver,
|
|
||||||
maximum_wait_seconds=.05, failed=lambda error: None)
|
|
||||||
try:
|
|
||||||
dispatcher.feedback_available(100)
|
|
||||||
dispatcher.submit(observation(1))
|
|
||||||
assert running.wait(1.)
|
|
||||||
for stamp in range(2, 100):
|
|
||||||
dispatcher.submit(observation(stamp))
|
|
||||||
# Feedback and submissions cannot wait for blocked geometry or the
|
|
||||||
# coordinator lock held by that geometry's consumer.
|
|
||||||
dispatcher.feedback_available(101)
|
|
||||||
release.set()
|
|
||||||
assert finished.wait(1.)
|
|
||||||
assert images == [1, 99]
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
dispatcher.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_start_or_abort_discards_images_waiting_for_feedback():
|
|
||||||
delivered = Event()
|
|
||||||
dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(),
|
|
||||||
maximum_wait_seconds=1., failed=lambda error: None)
|
|
||||||
dispatcher.submit(observation(10))
|
|
||||||
assert not delivered.wait(.02)
|
|
||||||
dispatcher.clear()
|
|
||||||
dispatcher.feedback_available(11)
|
|
||||||
dispatcher.close()
|
|
||||||
assert not delivered.is_set()
|
|
||||||
|
|
||||||
|
|
||||||
def test_new_session_cannot_use_the_previous_sessions_feedback_watermark():
|
|
||||||
delivered = Event()
|
|
||||||
dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(),
|
|
||||||
maximum_wait_seconds=1., failed=lambda error: None)
|
|
||||||
try:
|
|
||||||
dispatcher.feedback_available(100)
|
|
||||||
dispatcher.clear()
|
|
||||||
dispatcher.submit(observation(20))
|
|
||||||
assert not delivered.wait(.02)
|
|
||||||
dispatcher.feedback_available(21)
|
|
||||||
assert delivered.wait(.2)
|
|
||||||
finally:
|
|
||||||
dispatcher.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_missing_feedback_has_a_bounded_wait_and_capture_still_decides_validity():
|
|
||||||
delivered = Event()
|
|
||||||
dispatcher = DetectionDispatcher(('front',), lambda row: delivered.set(),
|
|
||||||
maximum_wait_seconds=.02, failed=lambda error: None)
|
|
||||||
try:
|
|
||||||
dispatcher.submit(observation(10))
|
|
||||||
assert delivered.wait(.5)
|
|
||||||
finally:
|
|
||||||
dispatcher.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_worker_errors_reach_the_control_thread():
|
|
||||||
failed = Event()
|
|
||||||
def fail(_):
|
|
||||||
raise ValueError('unexpected geometry error')
|
|
||||||
dispatcher = DetectionDispatcher(('front',), fail,
|
|
||||||
maximum_wait_seconds=.02, failed=lambda error: failed.set())
|
|
||||||
try:
|
|
||||||
dispatcher.submit(observation(10))
|
|
||||||
assert failed.wait(1.)
|
|
||||||
with pytest.raises(RuntimeError, match='image dispatch failed') as error:
|
|
||||||
dispatcher.raise_if_failed()
|
|
||||||
assert isinstance(error.value.__cause__, ValueError)
|
|
||||||
finally:
|
|
||||||
dispatcher.close()
|
|
||||||
@@ -1,255 +0,0 @@
|
|||||||
"""Detached analysis boundaries and source integrity; expensive fitting is mocked."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose import motion_image_diagnostics
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import MotionImageDiagnosticReport
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime import diagnostic_analysis as analysis
|
|
||||||
from test_recorded_diagnostic_replay import recorded_fixture
|
|
||||||
|
|
||||||
|
|
||||||
def write_actual_capture(directory):
|
|
||||||
fixture = recorded_fixture("192637")
|
|
||||||
records = [fixture["session_start"], fixture["camera_model"], *fixture["records"],
|
|
||||||
fixture["recorded_initialization"]]
|
|
||||||
path = directory / "raw_samples.jsonl"
|
|
||||||
path.write_text("".join(json.dumps(row)+"\n" for row in records))
|
|
||||||
return fixture, records, path
|
|
||||||
|
|
||||||
|
|
||||||
def mock_image_analysis(monkeypatch, callback=None):
|
|
||||||
calls = []
|
|
||||||
def solve(frames, relations):
|
|
||||||
calls.append((frames, relations))
|
|
||||||
if callback is not None:
|
|
||||||
callback()
|
|
||||||
return MotionImageDiagnosticReport("complete", "", tuple(frame.evidence.stamp_ns for frame in frames))
|
|
||||||
monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images", solve)
|
|
||||||
return calls
|
|
||||||
|
|
||||||
|
|
||||||
def test_joint_chain_holdout_never_enters_old_stage_fitter(tmp_path, monkeypatch):
|
|
||||||
(tmp_path/'diagnostic_capture.json').write_text(json.dumps(dict(mode='o30_pinky_chain')))
|
|
||||||
(tmp_path/'raw_samples.jsonl').write_text('{"cycle":3}\n')
|
|
||||||
monkeypatch.setattr(analysis, 'analyze_capture', lambda *a, **kw:
|
|
||||||
pytest.fail('independent chain holdout must not enter per-stage fitting'))
|
|
||||||
result = analysis.finish_diagnostic_analysis(load_bundled_hand_profile('o30_right_18'), tmp_path)
|
|
||||||
assert result['status'] == 'pending_joint_chain_validation'
|
|
||||||
assert not result['publication_allowed'] and not result['accuracy_validated']
|
|
||||||
assert result['training_cycles'] == [0, 1] and result['holdout_cycles'] == [3]
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("corrupt", [False, True])
|
|
||||||
def test_indexed_capture_is_decoded_and_corrupt_archive_is_rejected(tmp_path, monkeypatch, corrupt):
|
|
||||||
from linkerhand_calibration.core.artifacts.evidence_journal import (
|
|
||||||
archive_path, initialize_evidence_journal,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.storage import append_jsonl_many
|
|
||||||
|
|
||||||
fixture = recorded_fixture("192637")
|
|
||||||
rows = [fixture["session_start"], fixture["camera_model"], *fixture["records"],
|
|
||||||
fixture["recorded_initialization"]]
|
|
||||||
raw = tmp_path / "raw_samples.jsonl"
|
|
||||||
initialize_evidence_journal(raw, threshold_bytes=1)
|
|
||||||
append_jsonl_many(raw, rows)
|
|
||||||
archive = archive_path(raw)
|
|
||||||
if corrupt:
|
|
||||||
data = bytearray(archive.read_bytes())
|
|
||||||
data[0] ^= 1
|
|
||||||
archive.write_bytes(data)
|
|
||||||
before = (raw.read_bytes(), archive.read_bytes())
|
|
||||||
calls = mock_image_analysis(monkeypatch)
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
if corrupt:
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
analysis.analyze_capture(profile, raw)
|
|
||||||
assert not calls
|
|
||||||
assert not (tmp_path / "diagnostic_analysis.json").exists()
|
|
||||||
else:
|
|
||||||
report = analysis.analyze_capture(profile, raw)
|
|
||||||
assert report["status"] == "complete"
|
|
||||||
assert len(calls[0][0]) == 120
|
|
||||||
assert report["publication_allowed"] is False
|
|
||||||
assert (raw.read_bytes(), archive.read_bytes()) == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_actual_initialization_images_remain_bound_in_separate_analysis_output(tmp_path, monkeypatch):
|
|
||||||
fixture, _, raw = write_actual_capture(tmp_path)
|
|
||||||
before = raw.read_bytes()
|
|
||||||
(tmp_path / "diagnostic_capture.json").write_text('{"original": true}')
|
|
||||||
original = (tmp_path / "diagnostic_capture.json").read_bytes()
|
|
||||||
calls = mock_image_analysis(monkeypatch)
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
output = tmp_path / "analysis"
|
|
||||||
result = analysis.analyze_capture(profile, raw, output_dir=output)
|
|
||||||
assert len(calls) == 1
|
|
||||||
frames, relations = calls[0]
|
|
||||||
assert len(frames) == 120
|
|
||||||
assert [frame.evidence.stamp_ns for frame in frames] == fixture["recorded_initialization"]["source_image_stamps"]
|
|
||||||
assert {relation.joint for relation in relations} == {"rh_thumb_cmc_pitch", "rh_thumb_ip"}
|
|
||||||
assert result["raw_samples_sha256"] == hashlib.sha256(before).hexdigest()
|
|
||||||
assert result["publication_allowed"] is result["accuracy_validated"] is False
|
|
||||||
assert result["moving_candidates_counterfactual"] is True
|
|
||||||
assert result["stages"][0]["recorded_branch_status"] == "unresolved"
|
|
||||||
assert result["stages"][0]["source_image_count"] == 120
|
|
||||||
assert json.loads((output / "diagnostic_analysis.json").read_text())["publication_allowed"] is False
|
|
||||||
assert (output / "diagnostic_analysis.md").exists()
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
assert (tmp_path / "diagnostic_capture.json").read_bytes() == original
|
|
||||||
assert not list(tmp_path.rglob("*.urdf"))
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["second_session", "profile", "image_content", "source_identity", "duplicate_image"])
|
|
||||||
def test_bad_capture_identity_is_rejected_before_any_math_or_output(tmp_path, monkeypatch, fault):
|
|
||||||
_, rows, raw = write_actual_capture(tmp_path)
|
|
||||||
if fault == "second_session":
|
|
||||||
rows.append(deepcopy(rows[0]))
|
|
||||||
elif fault == "profile":
|
|
||||||
rows[0]["profile_id"] = "O12/right/foreign/v1"
|
|
||||||
elif fault == "image_content":
|
|
||||||
rows[2]["tags"]["thumb_ip"]["corners_xy"][0][0] += 1.
|
|
||||||
elif fault == "source_identity":
|
|
||||||
rows[2]["session_epoch"] += 1
|
|
||||||
else:
|
|
||||||
rows.append(deepcopy(rows[2]))
|
|
||||||
rows[-1]["tags"]["thumb_ip"]["corners_xy"][0][0] += 1.
|
|
||||||
raw.write_text("".join(json.dumps(row)+"\n" for row in rows))
|
|
||||||
before = raw.read_bytes()
|
|
||||||
monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images",
|
|
||||||
lambda *args, **kwargs: pytest.fail("invalid evidence reached image solver"))
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_analysis_"):
|
|
||||||
analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw)
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
assert not (tmp_path / "diagnostic_analysis.json").exists()
|
|
||||||
|
|
||||||
|
|
||||||
def test_frozen_root_is_preserved_while_moving_candidates_remain_counterfactual():
|
|
||||||
fixture = recorded_fixture("192637")
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
rows = [fixture["session_start"], *fixture["records"], fixture["recorded_initialization"]]
|
|
||||||
_, stages = analysis._stages(profile, rows)
|
|
||||||
_, relations, records = stages[0]
|
|
||||||
records = deepcopy(records)
|
|
||||||
for row in records:
|
|
||||||
for role in ("thumb_pitch", "thumb_ip"):
|
|
||||||
row["tags"][role]["candidate_diagnostics"]["branch_frozen"] = True
|
|
||||||
before = deepcopy(records)
|
|
||||||
frames = analysis.image_frames(records, relations)
|
|
||||||
for frame, original in zip(frames, records, strict=True):
|
|
||||||
poses = {item.role: item for item in frame.evidence.roles}
|
|
||||||
root = poses["front_base"]
|
|
||||||
assert root.frozen is True and len(root.candidates) == 1
|
|
||||||
assert root.candidates[0].quaternion_xyzw == tuple(original["tags"]["front_base"]["selected_pose"]["quaternion_xyzw"])
|
|
||||||
for role in ("thumb_pitch", "thumb_ip"):
|
|
||||||
assert poses[role].frozen is False
|
|
||||||
assert len(poses[role].candidates) == len(original["tags"][role]["reprojection_valid_candidates"])
|
|
||||||
assert records == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_source_change_during_analysis_prevents_publication_of_diagnostic_output(tmp_path, monkeypatch):
|
|
||||||
_, _, raw = write_actual_capture(tmp_path)
|
|
||||||
calls = mock_image_analysis(monkeypatch, lambda: raw.write_text(raw.read_text()+"\n"))
|
|
||||||
with pytest.raises(ValueError, match="source_changed_during_read"):
|
|
||||||
analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw)
|
|
||||||
assert len(calls) == 1
|
|
||||||
assert not (tmp_path / "diagnostic_analysis.json").exists()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("error_type", [ValueError, RuntimeError])
|
|
||||||
def test_failed_analysis_preserves_completed_capture_and_never_claims_accuracy(tmp_path, monkeypatch, error_type):
|
|
||||||
_, _, raw = write_actual_capture(tmp_path)
|
|
||||||
before = raw.read_bytes()
|
|
||||||
capture = tmp_path / "diagnostic_capture.json"
|
|
||||||
capture.write_text('{"result": "DIAGNOSTIC_COMPLETE"}')
|
|
||||||
original = capture.read_bytes()
|
|
||||||
def fail(*args, **kwargs):
|
|
||||||
raise error_type("test_analysis_failure")
|
|
||||||
monkeypatch.setattr(analysis, "analyze_capture", fail)
|
|
||||||
report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path)
|
|
||||||
assert report["status"] == "failed"
|
|
||||||
assert report["accuracy_validated"] is report["publication_allowed"] is False
|
|
||||||
assert raw.read_bytes() == before and capture.read_bytes() == original
|
|
||||||
assert not list(tmp_path.rglob("*.urdf"))
|
|
||||||
|
|
||||||
|
|
||||||
def test_unrelated_camera_sweep_is_not_reported_as_failed_joint_geometry(tmp_path, monkeypatch):
|
|
||||||
_, rows, raw = write_actual_capture(tmp_path)
|
|
||||||
unrelated = deepcopy(rows[2])
|
|
||||||
unrelated.update(kind="diagnostic_observation_frame", sample_phase="sweep",
|
|
||||||
task_name="thumb_yaw_top", cycle=0, direction="decreasing", attempt=1)
|
|
||||||
unrelated["image_stamp_ns"] += 1
|
|
||||||
for item in unrelated["tags"].values():
|
|
||||||
item["candidate_diagnostics"]["observation_stamp_ns"] = unrelated["image_stamp_ns"]
|
|
||||||
# The front camera was running for a later task in this same diagnostic;
|
|
||||||
# it has no measurement relation in the top-view yaw task.
|
|
||||||
rows.append(unrelated)
|
|
||||||
raw.write_text("".join(json.dumps(row)+"\n" for row in rows))
|
|
||||||
calls = mock_image_analysis(monkeypatch)
|
|
||||||
report = analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw)
|
|
||||||
assert len(calls) == len(report["stages"]) == 1
|
|
||||||
assert report["stages"][0]["task_name"] == "thumb_pitch_ip_front"
|
|
||||||
|
|
||||||
|
|
||||||
def test_fourth_round_sweep_never_enters_diagnostic_training(tmp_path, monkeypatch):
|
|
||||||
fixture = recorded_fixture("175815")
|
|
||||||
rows = [fixture["session_start"], *fixture["records"]]
|
|
||||||
for row in rows[1:]:
|
|
||||||
row["cycle"] = 3
|
|
||||||
raw = tmp_path / "raw_samples.jsonl"
|
|
||||||
raw.write_text("".join(json.dumps(row)+"\n" for row in rows))
|
|
||||||
before = raw.read_bytes()
|
|
||||||
monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images",
|
|
||||||
lambda *args, **kwargs: pytest.fail("holdout images reached optimizer"))
|
|
||||||
with pytest.raises(ValueError, match="refuses_holdout_training"):
|
|
||||||
analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw)
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("collision", ["filename", "symlink", "hardlink"])
|
|
||||||
def test_analysis_outputs_cannot_replace_raw_input(tmp_path, monkeypatch, collision):
|
|
||||||
import os
|
|
||||||
|
|
||||||
_, _, raw = write_actual_capture(tmp_path)
|
|
||||||
before = raw.read_bytes()
|
|
||||||
target = tmp_path / "diagnostic_analysis.md"
|
|
||||||
if collision == "filename":
|
|
||||||
raw.rename(target)
|
|
||||||
raw = target
|
|
||||||
elif collision == "symlink":
|
|
||||||
target.symlink_to(raw)
|
|
||||||
else:
|
|
||||||
os.link(raw, target)
|
|
||||||
monkeypatch.setattr(motion_image_diagnostics, "analyze_motion_images",
|
|
||||||
lambda *args, **kwargs: pytest.fail("output collision reached optimizer"))
|
|
||||||
with pytest.raises(ValueError, match="output_overwrites_source"):
|
|
||||||
analysis.analyze_capture(load_bundled_hand_profile("o6_right_8"), raw)
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_failure_report_cannot_bypass_output_source_collision_guard(tmp_path):
|
|
||||||
_, _, raw = write_actual_capture(tmp_path)
|
|
||||||
before = raw.read_bytes()
|
|
||||||
(tmp_path / "diagnostic_analysis.md").symlink_to(raw)
|
|
||||||
report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path)
|
|
||||||
assert report["status"] == "failed"
|
|
||||||
assert report["accuracy_validated"] is report["publication_allowed"] is False
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_failure_report_write_error_does_not_escape_into_capture_failure(tmp_path, monkeypatch, capsys):
|
|
||||||
_, _, raw = write_actual_capture(tmp_path)
|
|
||||||
before = raw.read_bytes()
|
|
||||||
def fail(*args, **kwargs):
|
|
||||||
raise OSError("diagnostic_output_unwritable")
|
|
||||||
monkeypatch.setattr(analysis, "analyze_capture", fail)
|
|
||||||
monkeypatch.setattr(analysis, "atomic_write_json", fail)
|
|
||||||
report = analysis.finish_diagnostic_analysis(load_bundled_hand_profile("o6_right_8"), tmp_path)
|
|
||||||
assert report["status"] == "failed"
|
|
||||||
assert report["publication_allowed"] is report["accuracy_validated"] is False
|
|
||||||
assert raw.read_bytes() == before
|
|
||||||
assert "原始采集状态保持不变" in capsys.readouterr().out
|
|
||||||
@@ -1,401 +0,0 @@
|
|||||||
"""Real coordinator diagnostics with projected Tags and an in-memory SDK clock."""
|
|
||||||
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
from dataclasses import replace
|
|
||||||
from pathlib import Path
|
|
||||||
from threading import Event
|
|
||||||
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.pnp import square_object_points
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.tracking import SquareTagPoseTracker
|
|
||||||
from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection
|
|
||||||
from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
|
|
||||||
|
|
||||||
THUMB_JOINTS = {"rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_thumb_ip"}
|
|
||||||
TERMINAL_PHASES = {Phase.DIAGNOSTIC_COMPLETE, Phase.PAUSED, Phase.ABORTED, Phase.FAILED}
|
|
||||||
|
|
||||||
|
|
||||||
def projected_frame(host, view, feedback, *, stationary_centers=False):
|
|
||||||
"""Project fixed mounts on two synthetic revolute chains.
|
|
||||||
|
|
||||||
The previous transport fixture rotated each Tag about its own fixed centre.
|
|
||||||
That is deliberately retained below as an unobservable counterexample. The
|
|
||||||
complete-flow fixture now supplies the physical moving-centre evidence that
|
|
||||||
pre-freeze branch confirmation requires; no recorded baseline is rewritten.
|
|
||||||
"""
|
|
||||||
yaw = (255 - feedback[1]) / 255 * .8
|
|
||||||
pitch = (255 - feedback[0]) / 255 * .9
|
|
||||||
ip = (255 - feedback[0]) / 255 * .5
|
|
||||||
base_rotation = Rotation.from_euler("xyz", [.2, .1, 0])
|
|
||||||
base_center = np.asarray([-.07, -.045, .4])
|
|
||||||
yaw_rotation = base_rotation * Rotation.from_rotvec([0, 0, yaw])
|
|
||||||
pitch_rotation = base_rotation * Rotation.from_rotvec([0, 0, pitch])
|
|
||||||
ip_rotation = pitch_rotation * Rotation.from_rotvec([-ip, 0, 0])
|
|
||||||
pitch_joint = base_center + base_rotation.apply([0.015, 0.02, 0])
|
|
||||||
ip_joint = pitch_joint + pitch_rotation.apply([0.06, 0, 0])
|
|
||||||
mounts = {
|
|
||||||
"thumb_yaw": (yaw_rotation, base_center + yaw_rotation.apply([.06, 0, 0])),
|
|
||||||
"thumb_pitch": (pitch_rotation, pitch_joint + pitch_rotation.apply([.03, 0, 0])),
|
|
||||||
"thumb_ip": (ip_rotation, ip_joint + ip_rotation.apply([0, .04, 0])),
|
|
||||||
}
|
|
||||||
detections = []
|
|
||||||
for index, tag in enumerate(next(v for v in host.profile.vision.views if v.name == view).tags):
|
|
||||||
if not tag.fixed_reference and tag.role not in mounts:
|
|
||||||
continue # Unselected pinky Tags stay hidden throughout the test.
|
|
||||||
rotation, center = mounts.get(tag.role, (base_rotation, base_center))
|
|
||||||
if stationary_centers:
|
|
||||||
center = np.asarray([-.07 + .07 * index, -.045 + .025 * index, .4])
|
|
||||||
rvec = rotation.as_rotvec()
|
|
||||||
corners, _ = cv2.projectPoints(square_object_points(tag.size_m), rvec, center,
|
|
||||||
host.cameras.matrices[view], np.zeros(5))
|
|
||||||
detections.append(TagDetection(tag.tag_id,
|
|
||||||
tuple(tuple(float(value) for value in row) for row in corners.reshape(4, 2)), 0, 100.))
|
|
||||||
return DetectionInput(view, host.ports.clock_ns(), tuple(detections))
|
|
||||||
|
|
||||||
|
|
||||||
def start_diagnostic(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
ready(host, clock)
|
|
||||||
for view in set(host.profile.vision.view_names) - set(host.required_observation_views):
|
|
||||||
for mapping in (host.cameras.matrices, host.cameras.image_sizes,
|
|
||||||
host.cameras.info_received_at, host.cameras.detections_received_at):
|
|
||||||
mapping.pop(view, None)
|
|
||||||
for view in host.required_observation_views:
|
|
||||||
host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]])
|
|
||||||
monkeypatch.setattr(host.finalization, "start", lambda *a, **kw: pytest.fail("diagnostic started finalizer"))
|
|
||||||
assert host.start().success
|
|
||||||
return host, clock
|
|
||||||
|
|
||||||
|
|
||||||
def advance(host, clock, *, images=True):
|
|
||||||
clock.now += .04
|
|
||||||
feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values
|
|
||||||
host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns())
|
|
||||||
host.tick()
|
|
||||||
if images:
|
|
||||||
task = next((task for task in host.profile.motion.tasks
|
|
||||||
if host._motion is not None and task.key == host._motion.task_key), None)
|
|
||||||
views = (task.view,) if task is not None else host.required_observation_views
|
|
||||||
for view in views:
|
|
||||||
host.receive_detections(projected_frame(host, view, feedback))
|
|
||||||
|
|
||||||
|
|
||||||
def run_until(host, clock, predicate, *, images=True):
|
|
||||||
for _ in range(8000):
|
|
||||||
if predicate():
|
|
||||||
return
|
|
||||||
if host.execution.session.phase in TERMINAL_PHASES:
|
|
||||||
pytest.fail(f"premature terminal state: {host.state}: {host.reason}")
|
|
||||||
advance(host, clock, images=images)
|
|
||||||
pytest.fail(f"virtual capture timed out: {host.state}: {host.reason}")
|
|
||||||
|
|
||||||
|
|
||||||
def assert_terminal_is_idle(host, clock, monkeypatch):
|
|
||||||
before = (len(clock.positions), len(clock.settings), len(host.raw_records), host.raw_path.read_bytes())
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: pytest.fail("terminal observation consumed"))
|
|
||||||
for _ in range(5):
|
|
||||||
advance(host, clock)
|
|
||||||
assert (len(clock.positions), len(clock.settings), len(host.raw_records), host.raw_path.read_bytes()) == before
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("diagnostic", [True, False])
|
|
||||||
def test_legacy_3d_stationary_centers_keep_raw_without_confirming_zero(tmp_path, monkeypatch, diagnostic):
|
|
||||||
import sys
|
|
||||||
|
|
||||||
original = projected_frame
|
|
||||||
monkeypatch.setattr(sys.modules[__name__], "projected_frame",
|
|
||||||
lambda host, view, feedback: original(host, view, feedback, stationary_centers=True))
|
|
||||||
if diagnostic:
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
else:
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
ready(host, clock)
|
|
||||||
for view in host.required_observation_views:
|
|
||||||
host.cameras.matrices[view] = np.asarray([[1000., 0., 320.], [0., 1000., 240.], [0., 0., 1.]])
|
|
||||||
assert host.start().success
|
|
||||||
# Historical 3D candidate geometry requires center translation. The new
|
|
||||||
# image solver uses all corners and may observe rotation around a Tag's
|
|
||||||
# center; this test retains the old diagnostic/compatibility contract.
|
|
||||||
host.branch_initialization.image_geometry = False
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES)
|
|
||||||
if diagnostic:
|
|
||||||
assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE
|
|
||||||
assert set(host.execution.diagnostic_zero_attempts) == THUMB_JOINTS
|
|
||||||
attempts = [row for row in host.raw_records if row.get("kind") == "diagnostic_joint_zero_attempt"]
|
|
||||||
assert len(attempts) == 2 # One visit per group across both directions.
|
|
||||||
assert all(row["no_zero_locked"] and not row["is_accepted_joint_sample"] for row in attempts)
|
|
||||||
assert len(host.execution.completed_units) == 4
|
|
||||||
else:
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert host.reason == "joint_zero_motion_unresolved:view=top:joints=rh_thumb_cmc_yaw:motion_geometry_unresolved"
|
|
||||||
assert not host.execution.diagnostic_zero_attempts
|
|
||||||
assert not host.execution.zero_references
|
|
||||||
reports = [r for r in host.raw_records if r.get("kind") == "motion_branch_initialization"]
|
|
||||||
assert reports and all(r["status"] == "unresolved" for r in reports)
|
|
||||||
assert any(q["reason"] == "insufficient_pivot_translation"
|
|
||||||
for r in reports for h in r["hypotheses"] for q in h["relations"])
|
|
||||||
assert not any(row.get("kind") == "joint_sample" for row in host.raw_records)
|
|
||||||
assert not host.finalization.started and not host.final_json and not host.final_urdf
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("unused_side", ["absent", "ambiguous_recorded_tag"])
|
|
||||||
def test_complete_thumb_diagnostic_uses_real_capture_and_never_publishes(tmp_path, monkeypatch, unused_side):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
from linkerhand_calibration.core.domain.motion_path import record_scan_identity
|
|
||||||
evaluate = host.execution.evaluate
|
|
||||||
evaluated_windows = []
|
|
||||||
def evaluate_window(records, **kwargs):
|
|
||||||
assert records is host._unit_rows
|
|
||||||
unit = host.execution.action.scan_unit
|
|
||||||
assert all(record_scan_identity(row) == unit.identity for row in records
|
|
||||||
if row.get("sample_phase") == "sweep")
|
|
||||||
evaluated_windows.append(unit.identity)
|
|
||||||
return evaluate(records, **kwargs)
|
|
||||||
monkeypatch.setattr(host.execution, "evaluate", evaluate_window)
|
|
||||||
# This historical transport fixture assumes its 3D PnP hinge solution is
|
|
||||||
# available; its nearly frontal mirror families need not be identifiable
|
|
||||||
# by the new image optimizer. Preserve the original capture/fault contract
|
|
||||||
# here. test_image_motion_live_flow covers the complete production image
|
|
||||||
# path with recorded preparation and directional observations.
|
|
||||||
host.branch_initialization.image_geometry = False
|
|
||||||
if unused_side == "ambiguous_recorded_tag":
|
|
||||||
fixture = json.loads((Path(__file__).with_name("fixtures") /
|
|
||||||
"o6_side_base_20260914_190619.json").read_text())
|
|
||||||
item = fixture["observation"]["pnp_observation_evidence"]
|
|
||||||
matrix = np.asarray(fixture["camera_model"]["camera_matrix"])
|
|
||||||
corners = item["corners_xy"]
|
|
||||||
# First reproduce the real stationary ambiguity with the unchanged
|
|
||||||
# tracker. Side transport then delivers the same corners during this
|
|
||||||
# run, but must never become a reference dependency of the thumb.
|
|
||||||
tracker = SquareTagPoseTracker()
|
|
||||||
for i in range(12):
|
|
||||||
pose, reason = tracker.estimate("side_base", corners,
|
|
||||||
tag_size_m=item["tag_size_m"], camera_matrix=matrix, stamp_ns=(i+1)*40_000_000)
|
|
||||||
assert pose is None and reason == "pose_branch_unconfirmed"
|
|
||||||
assert tracker.observation_snapshot("side_base", 12*40_000_000)[1]["branch_revision"] == 0
|
|
||||||
receive = host.receive_detections
|
|
||||||
def with_unused_side(frame):
|
|
||||||
receive(DetectionInput("side", frame.stamp_ns,
|
|
||||||
(TagDetection(3, tuple(tuple(p) for p in corners), 0, 100.),)))
|
|
||||||
receive(frame)
|
|
||||||
monkeypatch.setattr(host, "receive_detections", with_unused_side)
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE)
|
|
||||||
assert set(host.execution.zero_references) == THUMB_JOINTS
|
|
||||||
zero_rows = [row for row in host.raw_records if row.get("kind") == "joint_zero_reference"]
|
|
||||||
assert len(zero_rows) == 3
|
|
||||||
sweeps = [row for row in host.raw_records if row.get("kind") == "joint_sample"]
|
|
||||||
assert sweeps
|
|
||||||
assert {row["joint"] for row in sweeps} == THUMB_JOINTS
|
|
||||||
assert {row["sample_phase"] for row in sweeps} == {"sweep"}
|
|
||||||
assert {row["cycle"] for row in sweeps} == {0}
|
|
||||||
assert {row["direction"] for row in sweeps} == {"increasing", "decreasing"}
|
|
||||||
assert all(row["pnp_observation_evidence"] for row in sweeps)
|
|
||||||
assert len(host.execution.completed_units) == 4
|
|
||||||
assert set(evaluated_windows) == host.execution.completed_units
|
|
||||||
assert all(tuple(command[2:]) == host.profile.command.baseline_values[2:] for command in clock.positions)
|
|
||||||
assert host.last_command == host.profile.command.baseline_values
|
|
||||||
assert host.latest_feedback == host.profile.command.baseline_values
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
assert not list(tmp_path.rglob("*.urdf"))
|
|
||||||
assert not any(path.is_symlink() for path in tmp_path.rglob("*"))
|
|
||||||
report_path = host.parameters.session_dir / "diagnostic_capture.json"
|
|
||||||
report_text = report_path.read_text()
|
|
||||||
report = json.loads(report_text)
|
|
||||||
assert report["result"] == "DIAGNOSTIC_COMPLETE"
|
|
||||||
assert report["publication_allowed"] is False
|
|
||||||
assert report["accuracy_validated"] is False
|
|
||||||
assert report["raw_samples_sha256"] == hashlib.sha256(host.raw_path.read_bytes()).hexdigest()
|
|
||||||
assert set(report["joint_zero_references"]) == THUMB_JOINTS
|
|
||||||
assert len(report["completed_units"]) == 4
|
|
||||||
assert len(report["unit_quality"]) == 4
|
|
||||||
assert all(row["passed"] and row["accepted_pose_quality"]["passed"]
|
|
||||||
for row in report["unit_quality"])
|
|
||||||
assert report["accepted_pose_quality_incomplete_attempts"] == 0
|
|
||||||
assert report["pose_step_summary"]
|
|
||||||
assert {key: report["protected_inputs"][key] for key in host.parameters.protected_inputs} == host.parameters.protected_inputs
|
|
||||||
from linkerhand_calibration.core.artifacts.storage import load_jsonl
|
|
||||||
captured = load_jsonl(host.raw_path)
|
|
||||||
# A one-round thumb diagnostic proves its recorded references, not
|
|
||||||
# full-hand coverage or the formal four-round accuracy contract.
|
|
||||||
evidence = validate_motion_provenance(host.profile, captured, required=True)
|
|
||||||
assert evidence["motion_evidence_verified"] is True
|
|
||||||
assert evidence["is_accuracy_certificate"] is False
|
|
||||||
assert not host.capture_index.retain_complete
|
|
||||||
assert not host.capture_index.retain_training_geometry
|
|
||||||
assert not any(row.get("kind") in {"image_observation_frame",
|
|
||||||
"diagnostic_observation_frame", "motion_branch_observation"}
|
|
||||||
for row in host.capture_index)
|
|
||||||
locked = next(row for row in captured if row["kind"] == "fixed_base_reference_locked")
|
|
||||||
assert len([row for row in captured if row["kind"] == "fixed_base_reference_locked"]) == 1
|
|
||||||
assert set(locked["fixed_corners_by_view"]) == {"front", "top"}
|
|
||||||
assert set(locked["fixed_poses"]) == {"front", "top"}
|
|
||||||
assert set(host.capture.locked_reference_views()) == {"front", "top"}
|
|
||||||
assert not any(row.get("view") == "side" for row in captured)
|
|
||||||
assert set(host.trackers) == {"front", "top"}
|
|
||||||
start = next(row for row in captured if row["kind"] == "session_start")
|
|
||||||
assert start["required_observation_views"] == ["front", "top"]
|
|
||||||
assert report["protected_inputs"] == locked["protected_hashes"]
|
|
||||||
assert len(report["protected_inputs"]["intrinsics_sha256"]) == 64
|
|
||||||
snapshot = host.snapshot().calibration
|
|
||||||
assert snapshot.phase == "DIAGNOSTIC_COMPLETE"
|
|
||||||
assert snapshot.overall_progress_01 == 1.
|
|
||||||
assert snapshot.task.cycle_count == 1
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
assert not host.abort().success
|
|
||||||
assert not host.start().success
|
|
||||||
assert report_path.read_text() == report_text
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["ambiguous_pose", "missing_tag"])
|
|
||||||
def test_diagnostic_retains_rejected_candidates_but_missing_tag_still_retries_once(tmp_path, monkeypatch, fault):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
# This historical transport fixture assumes its 3D PnP hinge solution is
|
|
||||||
# available; its nearly frontal mirror families need not be identifiable
|
|
||||||
# by the new image optimizer. Preserve the original capture/fault contract
|
|
||||||
# here. test_image_motion_live_flow covers the complete production image
|
|
||||||
# path with recorded preparation and directional observations.
|
|
||||||
host.branch_initialization.image_geometry = False
|
|
||||||
commit = SquareTagPoseTracker.commit_motion_frame
|
|
||||||
|
|
||||||
def affected(role):
|
|
||||||
return role == "thumb_ip" and host._motion is not None and host._motion.phase == "sweep"
|
|
||||||
|
|
||||||
# Inject rejection at the new atomic delivery boundary after real PnP and
|
|
||||||
# geometric selection. The geometry tests separately exercise ambiguity;
|
|
||||||
# this transport test still requires retaining the original raw candidates.
|
|
||||||
def reject_motion(selections, **kwargs):
|
|
||||||
if any(affected(item.snapshot.role) for item in selections):
|
|
||||||
return SquareTagPoseTracker.reject_motion_frame(
|
|
||||||
tuple(item.snapshot for item in selections), "pose_branch_ambiguous")
|
|
||||||
return commit(selections, **kwargs)
|
|
||||||
|
|
||||||
receive = host.receive_detections
|
|
||||||
def hide_tag(frame):
|
|
||||||
if host._motion is not None and host._motion.phase == "sweep" and frame.view == "front":
|
|
||||||
frame = replace(frame, detections=tuple(tag for tag in frame.detections if tag.tag_id != 2))
|
|
||||||
return receive(frame)
|
|
||||||
|
|
||||||
if fault == "ambiguous_pose":
|
|
||||||
monkeypatch.setattr(SquareTagPoseTracker, "commit_motion_frame", staticmethod(reject_motion))
|
|
||||||
else:
|
|
||||||
monkeypatch.setattr(host, "receive_detections", hide_tag)
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES)
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
assert len([row for row in host.raw_records if row.get("kind") == "joint_zero_reference"]) == 3
|
|
||||||
assert not any(row.get("kind") == "joint_sample" and row.get("joint") == "rh_thumb_ip"
|
|
||||||
for row in host.raw_records)
|
|
||||||
checks = [row for row in host.raw_records if row.get("kind") == "scan_unit_complete"
|
|
||||||
and row["task_name"] == "thumb_pitch_ip_front"]
|
|
||||||
assert len(checks) == 2
|
|
||||||
assert all(row["accepted_pose_quality"]["passed"] is False for row in checks)
|
|
||||||
if fault == "missing_tag":
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert "sweep_quality_failed" in host.reason
|
|
||||||
assert host.snapshot().calibration.pause.details["observation_timing"] == checks[-1]["observation_timing"]
|
|
||||||
assert [row["attempt"] for row in checks] == [1, 2]
|
|
||||||
assert all(row["passed"] is False for row in checks)
|
|
||||||
assert not (host.parameters.session_dir / "diagnostic_capture.json").exists()
|
|
||||||
else:
|
|
||||||
assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE
|
|
||||||
assert [row["attempt"] for row in checks] == [1, 1]
|
|
||||||
assert all(row["passed"] for row in checks)
|
|
||||||
frames = [row for row in host.raw_records if row.get("kind") == "diagnostic_observation_frame"
|
|
||||||
and row["view"] == "front" and row["sample_phase"] == "sweep"]
|
|
||||||
assert frames and all(row["tags"]["thumb_ip"]["selected_pose"] is None for row in frames)
|
|
||||||
assert all(row["tags"]["thumb_ip"]["reprojection_valid_candidates"] for row in frames)
|
|
||||||
assert {row["direction"] for row in frames} == {"increasing", "decreasing"}
|
|
||||||
report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text())
|
|
||||||
assert report["accepted_pose_quality_incomplete_attempts"] == 2
|
|
||||||
assert not report["accuracy_validated"] and not report["publication_allowed"]
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("action", ["pause", "abort"])
|
|
||||||
def test_interrupted_thumb_diagnostic_holds_without_success_report(tmp_path, monkeypatch, action):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host._motion is not None and host._motion.phase == "sweep"
|
|
||||||
and host.last_command[1] < 240)
|
|
||||||
if action == "pause":
|
|
||||||
host._pause("test_visual_fault")
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
else:
|
|
||||||
assert host.abort().success
|
|
||||||
assert host.execution.session.phase == Phase.ABORTED
|
|
||||||
assert host.last_command != host.profile.command.baseline_values
|
|
||||||
assert not (host.parameters.session_dir / "diagnostic_capture.json").exists()
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_report_write_failure_is_terminal_and_cannot_restart_motion(tmp_path, monkeypatch):
|
|
||||||
from linkerhand_calibration.runtime import coordinator as coordinator_module
|
|
||||||
|
|
||||||
original_write = coordinator_module.atomic_write_json
|
|
||||||
def write(path, payload):
|
|
||||||
if path.name == "diagnostic_capture.json":
|
|
||||||
raise OSError("synthetic report write failure")
|
|
||||||
return original_write(path, payload)
|
|
||||||
monkeypatch.setattr(coordinator_module, "atomic_write_json", write)
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase == Phase.FAILED)
|
|
||||||
assert "diagnostic_capture_report_failed:synthetic report write failure" == host.reason
|
|
||||||
assert not (host.parameters.session_dir / "diagnostic_capture.json").exists()
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_pose_started_before_diagnostic_completion_cannot_change_hashed_capture(tmp_path, monkeypatch):
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase == Phase.RETURN_BASELINE)
|
|
||||||
def consume(*args, **kwargs):
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(5)
|
|
||||||
return ({"kind": "late_sample"},), None
|
|
||||||
monkeypatch.setattr(host.capture, "consume", consume)
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(host.receive_detections,
|
|
||||||
DetectionInput("front", host.ports.clock_ns(), ()))
|
|
||||||
assert entered.wait(2)
|
|
||||||
epoch = host._observation_epoch
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE,
|
|
||||||
images=False)
|
|
||||||
assert host._observation_epoch > epoch
|
|
||||||
report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text())
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
release.set()
|
|
||||||
future.result(timeout=2)
|
|
||||||
assert all(row.get("kind") != "late_sample" for row in host.raw_records)
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
assert hashlib.sha256(before).hexdigest() == report["raw_samples_sha256"]
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
@@ -1,216 +0,0 @@
|
|||||||
"""Only the selected diagnostic's camera dependencies may gate its session."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import (
|
|
||||||
DiagnosticCapturePlan,
|
|
||||||
resolve_diagnostic_capture,
|
|
||||||
)
|
|
||||||
from linkerhand_calibration.runtime.inputs import CameraModelInput, DetectionInput
|
|
||||||
from linkerhand_calibration.runtime.observation_scope import required_observation_views
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase, CalibrationSession
|
|
||||||
from runtime_host_fixture import coordinator_fixture, ready
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("profile_name", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"])
|
|
||||||
def test_formal_sessions_keep_every_declared_camera(profile_name):
|
|
||||||
profile = load_bundled_hand_profile(profile_name)
|
|
||||||
assert required_observation_views(profile) == profile.vision.view_names
|
|
||||||
assert CalibrationSession(profile).required_observation_views == profile.vision.view_names
|
|
||||||
|
|
||||||
|
|
||||||
def test_o6_thumb_dependencies_are_front_and_top():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
plan = resolve_diagnostic_capture(profile, "o6_thumb")
|
|
||||||
assert required_observation_views(profile, plan) == ("front", "top")
|
|
||||||
assert CalibrationSession(profile, diagnostic_capture=plan).required_observation_views == ("front", "top")
|
|
||||||
|
|
||||||
|
|
||||||
def test_virtual_task_and_camera_names_follow_measurements_including_secondary_view():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
renamed = {"front": "inspect", "side": "oblique", "top": "overhead"}
|
|
||||||
views = {view.name: replace(view, name=renamed[view.name]) for view in profile.vision.views}
|
|
||||||
tasks = tuple(replace(task, key=f"virtual_{task.key}", view=renamed[task.view])
|
|
||||||
for task in reversed(profile.motion.tasks))
|
|
||||||
measurements = {name: replace(spec, view=renamed.get(spec.view, spec.view))
|
|
||||||
for name, spec in profile.measurement.measurements.items()}
|
|
||||||
yaw = "rh_thumb_cmc_yaw"
|
|
||||||
secondary = "yaw_from_oblique_camera"
|
|
||||||
measurements[secondary] = replace(measurements[yaw], joint=secondary, view="oblique")
|
|
||||||
virtual = replace(profile,
|
|
||||||
vision=replace(profile.vision, views=(views["side"], views["top"], views["front"]),
|
|
||||||
extrinsic_reference_view="inspect"),
|
|
||||||
motion=replace(profile.motion, tasks=tasks),
|
|
||||||
measurement=replace(profile.measurement, measurements=measurements,
|
|
||||||
cross_view_sources={yaw: secondary}))
|
|
||||||
task = next(task for task in tasks if yaw in task.joints)
|
|
||||||
plan = DiagnosticCapturePlan("virtual_selection", (task.key,))
|
|
||||||
# Order comes from the rig, while inclusion comes from both observations.
|
|
||||||
assert required_observation_views(virtual, plan) == ("oblique", "overhead")
|
|
||||||
assert required_observation_views(virtual) == ("oblique", "overhead", "inspect")
|
|
||||||
|
|
||||||
|
|
||||||
def _remove_camera(host, view):
|
|
||||||
for mapping in (host.cameras.matrices, host.cameras.image_sizes,
|
|
||||||
host.cameras.info_received_at, host.cameras.detections_received_at,
|
|
||||||
host.cameras.errors, host.cameras.models):
|
|
||||||
mapping.pop(view, None)
|
|
||||||
|
|
||||||
|
|
||||||
def _invalid_camera_info():
|
|
||||||
# A well-shaped model with a different identity from the protected cameras.
|
|
||||||
return CameraModelInput(640, 480,
|
|
||||||
(500., 0., 320., 0., 500., 240., 0., 0., 1.), (),
|
|
||||||
tuple(np.eye(3).flat),
|
|
||||||
(500., 0., 320., 0., 0., 500., 240., 0., 0., 0., 1., 0.))
|
|
||||||
|
|
||||||
|
|
||||||
def test_missing_side_pipeline_does_not_block_thumb_ready_or_start(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
_remove_camera(host, "side")
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase == CalibrationPhase.READY
|
|
||||||
devices = host._device_status(clock.now)
|
|
||||||
assert devices.ready
|
|
||||||
assert set(devices.cameras) == {"front", "top"}
|
|
||||||
assert host.start().success
|
|
||||||
assert host.reference_lock.fixed_tag_by_view == {"front": 0, "top": 6}
|
|
||||||
preflight = json.loads((host.parameters.session_dir / "camera_preflight.json").read_text())
|
|
||||||
assert set(preflight["configuration"]) == {"front", "top"}
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("view", ["front", "top"])
|
|
||||||
@pytest.mark.parametrize("fault", ["info_missing", "detections_stale", "invalid_configuration"])
|
|
||||||
def test_required_camera_fault_after_ready_blocks_start_without_motion(tmp_path, view, fault):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
if fault == "info_missing":
|
|
||||||
host.cameras.info_received_at.pop(view)
|
|
||||||
elif fault == "detections_stale":
|
|
||||||
host.cameras.detections_received_at[view] = clock.now - 2.01
|
|
||||||
else:
|
|
||||||
host.receive_camera_info(view, _invalid_camera_info())
|
|
||||||
assert not host.start().success
|
|
||||||
assert host.execution.session.phase == CalibrationPhase.WAIT_DEVICE
|
|
||||||
assert view in host.reason
|
|
||||||
assert not host.started
|
|
||||||
assert not clock.positions
|
|
||||||
assert not clock.settings
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("view", ["front", "top"])
|
|
||||||
def test_required_camera_model_changes_still_pause_started_diagnostic(tmp_path, view):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
host.receive_camera_info(view, _invalid_camera_info())
|
|
||||||
assert host.execution.session.phase == CalibrationPhase.PAUSED
|
|
||||||
assert host.reason.startswith(f"protected_camera_model_changed:{view}:")
|
|
||||||
assert not host.final_json and not host.final_urdf
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_side_callbacks_cannot_change_diagnostic_evidence_or_pause(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
host.reference_lock.start_locking()
|
|
||||||
corners = np.asarray([[10., 10.], [30., 10.], [30., 30.], [10., 30.]])
|
|
||||||
for view, tag in host.reference_lock.fixed_tag_by_view.items():
|
|
||||||
for _ in range(host.reference_lock.minimum_frames):
|
|
||||||
host.reference_lock.observe(view, tag, corners)
|
|
||||||
assert host.reference_lock.locked
|
|
||||||
fingerprint = host.reference_lock.fingerprint()
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
record_kinds = [row["kind"] for row in host.raw_records]
|
|
||||||
seen = dict(host.cameras.detections_received_at)
|
|
||||||
matrices = {view: matrix.copy() for view, matrix in host.cameras.matrices.items()}
|
|
||||||
errors = dict(host.cameras.errors)
|
|
||||||
phase = host.execution.session.phase
|
|
||||||
monkeypatch.setattr(host.capture, "consume",
|
|
||||||
lambda *args, **kwargs: pytest.fail("unselected side observation entered capture"))
|
|
||||||
clock.now += .1
|
|
||||||
host.receive_detections(DetectionInput("side", host.ports.clock_ns(), ()))
|
|
||||||
host.receive_camera_info("side", _invalid_camera_info())
|
|
||||||
assert host.execution.session.phase == phase
|
|
||||||
assert host.cameras.detections_received_at == seen
|
|
||||||
assert host.cameras.errors == errors
|
|
||||||
for view, matrix in matrices.items():
|
|
||||||
np.testing.assert_array_equal(host.cameras.matrices[view], matrix)
|
|
||||||
assert host.reference_lock.fingerprint() == fingerprint
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
assert [row["kind"] for row in host.raw_records] == record_kinds
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_intrinsics_fingerprint_ignores_unselected_cached_camera(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
host.reference_lock.start_locking()
|
|
||||||
corners = np.asarray([[10., 10.], [30., 10.], [30., 30.], [10., 30.]])
|
|
||||||
for view, tag in host.reference_lock.fixed_tag_by_view.items():
|
|
||||||
for _ in range(host.reference_lock.minimum_frames):
|
|
||||||
host.reference_lock.observe(view, tag, corners)
|
|
||||||
host._save_reference()
|
|
||||||
first = host._current_fingerprint
|
|
||||||
expected = hashlib.sha256(json.dumps({view: host.cameras.matrices[view].tolist()
|
|
||||||
for view in ("front", "top")}, sort_keys=True).encode()).hexdigest()
|
|
||||||
assert first["protected_hashes"]["intrinsics_sha256"] == expected
|
|
||||||
assert set(first["fixed_corners_by_view"]) == {"front", "top"}
|
|
||||||
# Cached data can predate Start; its presence cannot alter this session's identity.
|
|
||||||
host.cameras.matrices["side"] = 9. * np.eye(3)
|
|
||||||
host._save_reference()
|
|
||||||
assert host._current_fingerprint == first
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_formal_side_pipeline_remains_a_required_device(tmp_path):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
_remove_camera(host, "side")
|
|
||||||
host.tick()
|
|
||||||
assert host.execution.session.phase == CalibrationPhase.WAIT_DEVICE
|
|
||||||
assert "side" in host.reason
|
|
||||||
assert not host.start().success
|
|
||||||
assert set(host.reference_lock.fixed_tag_by_view) == {"front", "side", "top"}
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def test_formal_side_callbacks_still_enter_capture_and_check_camera_identity(tmp_path, monkeypatch):
|
|
||||||
host, clock = coordinator_fixture(tmp_path)
|
|
||||||
try:
|
|
||||||
ready(host, clock)
|
|
||||||
assert host.start().success
|
|
||||||
calls = []
|
|
||||||
monkeypatch.setattr(host.capture, "consume", lambda frame, *args, **kwargs: calls.append(frame) or ([], None))
|
|
||||||
clock.now += .1
|
|
||||||
host.receive_detections(DetectionInput("side", host.ports.clock_ns(), ()))
|
|
||||||
assert [frame.view for frame in calls] == ["side"]
|
|
||||||
assert host.cameras.detections_received_at["side"] == clock.now
|
|
||||||
host.receive_camera_info("side", _invalid_camera_info())
|
|
||||||
assert host.execution.session.phase == CalibrationPhase.PAUSED
|
|
||||||
assert host.reason.startswith("protected_camera_model_changed:side:")
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
@@ -1,128 +0,0 @@
|
|||||||
"""Named diagnostic presets keep motion bounded and cannot publish or resume."""
|
|
||||||
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.product import load_product_config
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime import runner
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture, reject_diagnostic_capture
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
|
|
||||||
|
|
||||||
PACKAGE = Path(__file__).resolve().parents[1]
|
|
||||||
|
|
||||||
|
|
||||||
def test_all_o30_tasks_cover_every_joint_without_extra_rounds():
|
|
||||||
from linkerhand_calibration.runtime.observation_scope import required_observation_views
|
|
||||||
from linkerhand_calibration.core.domain.capture_plan import configure_training, TASKWISE_TWO_ROUND
|
|
||||||
from linkerhand_calibration.profiles.observer_policy import configure_observer_policy
|
|
||||||
profile = configure_observer_policy(configure_training(
|
|
||||||
load_bundled_hand_profile('o30_right_18'), TASKWISE_TWO_ROUND))
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_all_tasks')
|
|
||||||
assert plan.task_keys == tuple(task.key for task in profile.motion.tasks)
|
|
||||||
assert len(plan.task_keys) == 17
|
|
||||||
assert {joint for task in profile.motion.tasks for joint in task.joints} == set(profile.zero.active_joints)
|
|
||||||
assert required_observation_views(profile, plan) == ('front', 'side', 'top')
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
assert len(units) == 36
|
|
||||||
assert {unit.cycle for unit in units} == {0}
|
|
||||||
assert not profile.motion.preparation_witnesses
|
|
||||||
assert not profile.motion.observer_preparations
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_capture_not_for_publication_or_resume'):
|
|
||||||
reject_diagnostic_capture([dict(kind='session_start', diagnostic_capture=plan.as_dict())])
|
|
||||||
|
|
||||||
|
|
||||||
def test_o30_pinky_preset_keeps_dependency_order_and_side_camera():
|
|
||||||
from linkerhand_calibration.runtime.observation_scope import required_observation_views
|
|
||||||
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_pinky_side')
|
|
||||||
assert plan.task_keys == ('pinky_mcp_pitch_side', 'pinky_pip_side', 'pinky_dip_side')
|
|
||||||
assert required_observation_views(profile, plan) == ('side',)
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
assert [(u.task_key, u.cycle, u.direction) for u in units] == [
|
|
||||||
(key, 0, direction) for key in plan.task_keys
|
|
||||||
for direction in ('increasing', 'decreasing')]
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_capture_not_for_publication_or_resume'):
|
|
||||||
reject_diagnostic_capture([dict(kind='session_start', diagnostic_capture=plan.as_dict())])
|
|
||||||
|
|
||||||
|
|
||||||
def test_o30_roll_preset_keeps_one_round_and_existing_clearance():
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_thumb_roll')
|
|
||||||
assert plan.task_keys == ('thumb_cmc_roll_front',)
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
assert [(u.task_key, u.cycle, u.direction) for u in units] == [
|
|
||||||
('thumb_cmc_roll_front', 0, 'increasing'), ('thumb_cmc_roll_front', 0, 'decreasing')]
|
|
||||||
task = profile.motion.tasks[0]
|
|
||||||
assert task.auxiliary_commands == ((2, 0),)
|
|
||||||
assert task.exit_waypoints == (((0, 0),), ((2, 255),))
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_capture_not_for_publication_or_resume'):
|
|
||||||
reject_diagnostic_capture([dict(kind='session_start', diagnostic_capture=plan.as_dict())])
|
|
||||||
|
|
||||||
|
|
||||||
def test_existing_finger_roll_adds_camera_not_motion():
|
|
||||||
from dataclasses import replace
|
|
||||||
from linkerhand_calibration.runtime.observation_scope import required_observation_views
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_finger_roll_views')
|
|
||||||
assert plan.task_keys == ('fingers_mcp_roll_front',)
|
|
||||||
assert plan.observer_views == ('side',)
|
|
||||||
assert required_observation_views(profile, plan) == ('front', 'side')
|
|
||||||
primary_only = replace(plan, observer_views=())
|
|
||||||
assert required_observation_views(profile, primary_only) == ('front',)
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
assert units == CalibrationEngine(profile, diagnostic_capture=primary_only).scan_units()
|
|
||||||
assert {unit.cycle for unit in units} == {0}
|
|
||||||
assert plan.as_dict()['observer_views'] == ('side',)
|
|
||||||
assert 'observer_views' not in primary_only.as_dict()
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_observation_views_invalid'):
|
|
||||||
required_observation_views(profile, replace(plan, observer_views=('unknown',)))
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_observer_views_invalid'):
|
|
||||||
replace(plan, observer_views=('side', 'side'))
|
|
||||||
with pytest.raises(ValueError, match='diagnostic_capture_not_for_publication_or_resume'):
|
|
||||||
reject_diagnostic_capture([dict(kind='session_start', diagnostic_capture=plan.as_dict())])
|
|
||||||
|
|
||||||
|
|
||||||
def test_named_diagnostic_cli_disables_resume_and_uses_common_runner(monkeypatch):
|
|
||||||
config = load_product_config(PACKAGE/'config/o30_right_product.yaml',
|
|
||||||
workspace=PACKAGE.parents[1], check_can=False)
|
|
||||||
monkeypatch.setattr(runner, 'load_product_config', lambda *args, **kwargs: config)
|
|
||||||
calls = []
|
|
||||||
monkeypatch.setattr(runner, 'run_online', lambda config, **kwargs: calls.append(kwargs) or 0)
|
|
||||||
with pytest.raises(SystemExit) as stopped:
|
|
||||||
runner.main(['--diagnostic-capture', 'o30_thumb_roll', '--training-policy', 'fixed'])
|
|
||||||
assert stopped.value.code == 0
|
|
||||||
assert calls == [dict(record_bag=False, commands_enabled=True, allow_resume=False,
|
|
||||||
diagnostic_capture='o30_thumb_roll')]
|
|
||||||
with pytest.raises(SystemExit) as stopped:
|
|
||||||
runner.main(['--diagnostic-capture', 'o6_thumb'])
|
|
||||||
assert stopped.value.code == 2
|
|
||||||
assert len(calls) == 1
|
|
||||||
|
|
||||||
|
|
||||||
def test_chain_cli_uses_two_plus_one_default_and_never_resumes(monkeypatch):
|
|
||||||
config = load_product_config(PACKAGE/'config/o30_right_product.yaml',
|
|
||||||
workspace=PACKAGE.parents[1], check_can=False)
|
|
||||||
monkeypatch.setattr(runner, 'load_product_config', lambda *args, **kwargs: config)
|
|
||||||
calls = []
|
|
||||||
monkeypatch.setattr(runner, 'run_online', lambda config, **kwargs: calls.append(kwargs) or 0)
|
|
||||||
with pytest.raises(SystemExit) as stopped:
|
|
||||||
runner.main(['--diagnostic-capture', 'o30_pinky_chain', '--training-policy', 'fixed'])
|
|
||||||
assert stopped.value.code == 2 and not calls
|
|
||||||
with pytest.raises(SystemExit) as stopped:
|
|
||||||
runner.main(['--diagnostic-capture', 'o30_pinky_chain'])
|
|
||||||
assert stopped.value.code == 0
|
|
||||||
assert calls == [dict(record_bag=False, commands_enabled=True, allow_resume=False,
|
|
||||||
diagnostic_capture='o30_pinky_chain')]
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize('other', [['--offline-raw', 'capture.jsonl'], ['--publish-offline'],
|
|
||||||
['--commands-disabled'], ['--diagnostic-thumb']])
|
|
||||||
def test_named_diagnostic_cli_rejects_conflicting_modes(monkeypatch, other):
|
|
||||||
monkeypatch.setattr(runner, 'run_online', lambda *args, **kwargs: pytest.fail('must not start hardware'))
|
|
||||||
with pytest.raises(SystemExit) as stopped:
|
|
||||||
runner.main(['--diagnostic-capture', 'o30_thumb_roll', *other])
|
|
||||||
assert stopped.value.code == 2
|
|
||||||
@@ -1,98 +0,0 @@
|
|||||||
"""A diagnostic thumb capture cannot become a production fit or checkpoint."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
import json
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION
|
|
||||||
from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session
|
|
||||||
from linkerhand_calibration.runtime.artifacts.replay import validate_capture_provenance
|
|
||||||
from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION
|
|
||||||
from linkerhand_calibration.runtime.resume import discover_resume_candidate
|
|
||||||
|
|
||||||
|
|
||||||
DIAGNOSTIC = {
|
|
||||||
"mode": "o6_thumb",
|
|
||||||
"task_keys": ["thumb_yaw_top", "thumb_pitch_ip_front"],
|
|
||||||
"cycles": [0],
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("marker", [DIAGNOSTIC, True, "o6_thumb"])
|
|
||||||
def test_offline_replay_rejects_diagnostic_header_before_other_provenance(marker):
|
|
||||||
rows = [{"kind": "session_start", "diagnostic_capture": marker}]
|
|
||||||
before = deepcopy(rows)
|
|
||||||
with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"):
|
|
||||||
validate_capture_provenance(None, "TEST", {}, rows)
|
|
||||||
assert rows == before
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("record", [
|
|
||||||
{"kind": "diagnostic_observation_frame"},
|
|
||||||
{"kind": "scan_unit_complete", "quality_scope": "diagnostic_observation_coverage"},
|
|
||||||
])
|
|
||||||
def test_removing_header_does_not_promote_diagnostic_records(record):
|
|
||||||
with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"):
|
|
||||||
validate_capture_provenance(None, "TEST", {}, [record])
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("publish", [False, True])
|
|
||||||
def test_direct_finalizer_rejects_diagnostic_before_files_or_fitting(tmp_path, publish):
|
|
||||||
source = tmp_path / "original.urdf"
|
|
||||||
source.write_text('<robot name="untouched"/>', encoding="utf-8")
|
|
||||||
original = source.read_bytes()
|
|
||||||
previous = tmp_path / "previous_release"
|
|
||||||
previous.mkdir()
|
|
||||||
pointer = tmp_path / "latest_passed"
|
|
||||||
pointer.symlink_to(previous, target_is_directory=True)
|
|
||||||
directory = tmp_path / "diagnostic_output"
|
|
||||||
rows = [{"kind": "session_start", "diagnostic_capture": DIAGNOSTIC}]
|
|
||||||
with pytest.raises(ValueError, match="^diagnostic_capture_not_for_publication_or_resume$"):
|
|
||||||
finalize_profile_session(profile=None, session_dir=directory,
|
|
||||||
serial_number="TEST", source_urdf=source, protected_inputs={},
|
|
||||||
records=rows, publish=publish,
|
|
||||||
standard_loader=lambda _: pytest.fail("diagnostic data must never reach a loader"))
|
|
||||||
assert not directory.exists()
|
|
||||||
assert source.read_bytes() == original
|
|
||||||
assert pointer.resolve() == previous
|
|
||||||
|
|
||||||
|
|
||||||
def _checkpoint(directory, *, diagnostic=False):
|
|
||||||
directory.mkdir()
|
|
||||||
header = {
|
|
||||||
"kind": "session_start", "profile_id": "O6/right/layout/v1",
|
|
||||||
"serial_number": "TEST", "acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
|
||||||
"capture_schedule_version": CAPTURE_SCHEDULE_VERSION,
|
|
||||||
"pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION,
|
|
||||||
"protected_hashes": {"source": "a" * 64},
|
|
||||||
}
|
|
||||||
if diagnostic:
|
|
||||||
header["diagnostic_capture"] = DIAGNOSTIC
|
|
||||||
records = [header, {"kind": "fixed_base_reference_locked"},
|
|
||||||
{"kind": "scan_unit_complete", "passed": True}]
|
|
||||||
path = directory / "raw_samples.jsonl"
|
|
||||||
path.write_text("\n".join(json.dumps(row) for row in records), encoding="utf-8")
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("production_available", [False, True])
|
|
||||||
def test_diagnostic_checkpoint_never_shadows_or_supplies_production_resume(tmp_path, production_available):
|
|
||||||
production = tmp_path / "01"
|
|
||||||
if production_available:
|
|
||||||
_checkpoint(production)
|
|
||||||
diagnostic = _checkpoint(tmp_path / "02", diagnostic=True)
|
|
||||||
before = diagnostic.read_bytes()
|
|
||||||
# Discovery must reject the raw header even if diagnostic_capture.json is
|
|
||||||
# absent and the recorded direction claims to have passed all quality gates.
|
|
||||||
found = discover_resume_candidate(tmp_path, profile_id="O6/right/layout/v1",
|
|
||||||
serial_number="TEST", protected_hashes={"source": "a" * 64})
|
|
||||||
assert found == (production if production_available else None)
|
|
||||||
assert diagnostic.read_bytes() == before
|
|
||||||
|
|
||||||
|
|
||||||
def test_resume_discovery_checks_diagnostic_evidence_beyond_header(tmp_path):
|
|
||||||
path = _checkpoint(tmp_path / "01")
|
|
||||||
path.write_text(path.read_text() + '\n' + json.dumps({"kind": "diagnostic_observation_frame"}))
|
|
||||||
assert discover_resume_candidate(tmp_path, profile_id="O6/right/layout/v1",
|
|
||||||
serial_number="TEST", protected_hashes={"source": "a" * 64}) is None
|
|
||||||
@@ -1,159 +0,0 @@
|
|||||||
"""Raw image coverage can finish a diagnosis but cannot certify joint poses."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_quality import evaluate_diagnostic_unit
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit
|
|
||||||
from linkerhand_calibration.core.domain.motion_path import task_segment
|
|
||||||
|
|
||||||
|
|
||||||
def raw_frames(profile, unit):
|
|
||||||
task = next(task for task in profile.motion.tasks if task.key == unit.task_key)
|
|
||||||
rows = []
|
|
||||||
for index in range(129):
|
|
||||||
feedback = list(profile.command.baseline_values)
|
|
||||||
feedback[task.command_index] = unit.start + (unit.end - unit.start) * index / 128
|
|
||||||
for view in profile.vision.views:
|
|
||||||
stamp = 1_000_000_000 + index * 40_000_000
|
|
||||||
rows.append({"kind": "diagnostic_observation_frame", "view": view.name,
|
|
||||||
"task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction,
|
|
||||||
"attempt": 1, "sample_phase": "sweep", "image_stamp_ns": stamp,
|
|
||||||
"feedback_vector": feedback, "command_vector": list(feedback),
|
|
||||||
"command_unit": profile.command.unit, "is_accepted_joint_sample": False,
|
|
||||||
"tags": {tag.role: {"tag_role": tag.role, "tag_id": tag.tag_id,
|
|
||||||
"corners_xy": [[10, 10], [50, 10], [50, 50], [10, 50]],
|
|
||||||
"selected_pose": None,
|
|
||||||
"candidate_diagnostics": {"observation_stamp_ns": stamp,
|
|
||||||
"branch_status": "frozen_ambiguous"}}
|
|
||||||
for tag in view.tags if not tag.fixed_reference}})
|
|
||||||
return rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19", "o30_right_18"])
|
|
||||||
def test_raw_corners_cover_motion_without_becoming_measured_joint_angles(layout):
|
|
||||||
profile = load_bundled_hand_profile(layout)
|
|
||||||
unit = CalibrationEngine(profile).scan_units()[0]
|
|
||||||
rows = raw_frames(profile, unit)
|
|
||||||
before = deepcopy(rows)
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert result.observations.passed, result.observations.failures
|
|
||||||
assert not result.accepted_poses.passed
|
|
||||||
assert result.accepted_pose_report()["accuracy_validated"] is False
|
|
||||||
assert not evaluate_capture_unit(profile, unit, 1, rows,
|
|
||||||
first_cycle_spans={}, include_steady=False).passed
|
|
||||||
assert rows == before
|
|
||||||
assert all("joint" not in row and "observation_joint" not in row for row in rows)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["missing", "parent_missing", "stale_image", "tag_id",
|
|
||||||
"tag_role", "corners", "feedback", "command", "unit", "view", "task", "cycle",
|
|
||||||
"direction", "attempt", "outside_range"])
|
|
||||||
def test_raw_coverage_still_requires_current_correctly_bound_moving_tags_and_feedback(fault):
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
unit = next(unit for unit in CalibrationEngine(profile).scan_units()
|
|
||||||
if unit.task_key == "thumb_pitch_ip_front")
|
|
||||||
rows = raw_frames(profile, unit)
|
|
||||||
for row in rows:
|
|
||||||
if row["view"] != "front":
|
|
||||||
continue
|
|
||||||
item = row["tags"]["thumb_ip"]
|
|
||||||
if fault == "missing":
|
|
||||||
del row["tags"]["thumb_ip"]
|
|
||||||
elif fault == "parent_missing":
|
|
||||||
del row["tags"]["thumb_pitch"]
|
|
||||||
elif fault == "stale_image":
|
|
||||||
item["candidate_diagnostics"]["observation_stamp_ns"] -= 1
|
|
||||||
elif fault in {"tag_id", "tag_role", "corners"}:
|
|
||||||
item[{"corners": "corners_xy"}.get(fault, fault)] = {
|
|
||||||
"tag_id": 1, "tag_role": "thumb_pitch", "corners": None}[fault]
|
|
||||||
elif fault == "outside_range":
|
|
||||||
row["feedback_vector"][0] = -1
|
|
||||||
else:
|
|
||||||
key, value = {"feedback": ("feedback_vector", None), "command": ("command_vector", []),
|
|
||||||
"unit": ("command_unit", "rad"), "view": ("view", "side"),
|
|
||||||
"task": ("task_name", "thumb_yaw_top"), "cycle": ("cycle", 1),
|
|
||||||
"direction": ("direction", "increasing"), "attempt": ("attempt", 2)}[fault]
|
|
||||||
row[key] = value
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert not result.observations.passed
|
|
||||||
assert not result.accepted_poses.passed
|
|
||||||
|
|
||||||
|
|
||||||
def test_visible_tags_and_full_endpoint_span_cannot_hide_a_synchronization_gap():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
unit = CalibrationEngine(profile).scan_units()[0]
|
|
||||||
rows = raw_frames(profile, unit)
|
|
||||||
for row in rows:
|
|
||||||
if row["view"] == "top" and 80 < row["feedback_vector"][1] < 130:
|
|
||||||
row["feedback_vector"] = None
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert not result.observations.passed
|
|
||||||
assert any("maximum_gap=" in reason for reason in result.observations.failures)
|
|
||||||
metrics = next(iter(result.observations.metrics.values()))
|
|
||||||
assert metrics["feedback_span"] == 1.
|
|
||||||
assert metrics["allowed_maximum_bin_gap"] == 16
|
|
||||||
timing = next(iter(result.observation_timing.values()))
|
|
||||||
assert timing["raw_frames"] == timing["visible_frames"] == 129
|
|
||||||
assert timing["missing_feedback_frames"] > 0
|
|
||||||
assert timing["maximum_synchronized_gap_seconds"] > timing["maximum_image_gap_seconds"]
|
|
||||||
|
|
||||||
|
|
||||||
def test_command_release_diagnostic_does_not_substitute_encoder_for_command():
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
unit = next(u for u in CalibrationEngine(profile).scan_units()
|
|
||||||
if u.task_key == 'pinky_dip_side')
|
|
||||||
rows = raw_frames(profile, unit)
|
|
||||||
channel = profile.command.command_index_by_joint['pinky_dip']
|
|
||||||
for row in rows:
|
|
||||||
row['command_vector'][channel] = 127.
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert not result.observations.passed
|
|
||||||
assert any('command_span' in reason for reason in result.observations.failures)
|
|
||||||
assert not result.accepted_poses.passed
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("phase", ["steady", "joint_zero"])
|
|
||||||
def test_non_sweep_diagnostic_images_cannot_fill_missing_sweep_coverage(phase):
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
unit = CalibrationEngine(profile).scan_units()[0]
|
|
||||||
rows = [dict(row, sample_phase=phase) for row in raw_frames(profile, unit)]
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert not result.observations.passed and not result.accepted_poses.passed
|
|
||||||
assert all(value["raw_frames"] == 0 for value in result.observation_timing.values())
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("segment_key", ["linked_increasing", "linked_decreasing",
|
|
||||||
"middle_lower_decreasing", "middle_lower_increasing"])
|
|
||||||
def test_grouped_diagnostic_uses_each_channel_and_segment_identity(segment_key):
|
|
||||||
profile = load_bundled_hand_profile("o30_right_18")
|
|
||||||
unit = next(u for u in CalibrationEngine(profile).scan_units()
|
|
||||||
if u.task_key == "fingers_mcp_roll_front" and u.segment_key == segment_key)
|
|
||||||
task = next(t for t in profile.motion.tasks if t.key == unit.task_key)
|
|
||||||
segment = task_segment(task, segment_key)
|
|
||||||
rows = raw_frames(profile, unit)
|
|
||||||
for row in rows:
|
|
||||||
progress = (row["image_stamp_ns"] - 1_000_000_000) / (128 * 40_000_000)
|
|
||||||
commands = list(profile.command.baseline_values)
|
|
||||||
for channel, start in segment.start_commands:
|
|
||||||
end = dict(segment.end_commands)[channel]
|
|
||||||
commands[channel] = start + progress * (end - start)
|
|
||||||
row.update(segment_key=segment_key, command_vector=commands,
|
|
||||||
feedback_vector=list(commands))
|
|
||||||
result = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert result.observations.passed, result.observations.failures
|
|
||||||
assert not result.accepted_poses.passed
|
|
||||||
assert len(result.observations.metrics) == len(segment.joints)
|
|
||||||
wrong_segment = [dict(row, segment_key="another_segment") for row in rows]
|
|
||||||
assert not evaluate_diagnostic_unit(profile, unit, 1, wrong_segment).observations.passed
|
|
||||||
# Another finger's full travel must not cover this finger's stopped encoder.
|
|
||||||
channel = profile.command.command_index_by_joint["middle_mcp_roll"]
|
|
||||||
for row in rows:
|
|
||||||
row["feedback_vector"][channel] = 80.
|
|
||||||
stopped = evaluate_diagnostic_unit(profile, unit, 1, rows)
|
|
||||||
assert not stopped.observations.passed
|
|
||||||
assert any("middle_mcp_roll" in reason and "below_motion_resolution" in reason
|
|
||||||
for reason in stopped.observations.failures)
|
|
||||||
@@ -1,218 +0,0 @@
|
|||||||
"""Raw diagnostics survive unobservable geometry without manufacturing zeros."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
from concurrent.futures import ThreadPoolExecutor
|
|
||||||
from dataclasses import replace
|
|
||||||
import json
|
|
||||||
from threading import Event
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionBranchResolution
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime import coordinator as coordinator_module
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_reference import build_diagnostic_zero_attempt
|
|
||||||
from linkerhand_calibration.runtime.execution import SessionExecution
|
|
||||||
from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
|
|
||||||
from test_diagnostic_coordinator import (start_diagnostic, run_until, TERMINAL_PHASES,
|
|
||||||
assert_terminal_is_idle, projected_frame)
|
|
||||||
|
|
||||||
|
|
||||||
FRONT_JOINTS = {"rh_thumb_cmc_pitch", "rh_thumb_ip"}
|
|
||||||
|
|
||||||
|
|
||||||
def unresolved_front(monkeypatch):
|
|
||||||
solve = coordinator_module.solve_initialization
|
|
||||||
monkeypatch.setattr(coordinator_module, "solve_initialization", lambda request:
|
|
||||||
MotionBranchResolution(False, "motion_geometry_unresolved")
|
|
||||||
if request.view == "front" else solve(request))
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["geometry_only", "missing_sweep_tag", "missing_zero_tag", "unstable_feedback"])
|
|
||||||
def test_unresolved_reference_diagnostic_obeys_raw_coverage_and_sdk_stability(tmp_path, monkeypatch, fault):
|
|
||||||
unresolved_front(monkeypatch)
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
receive = host.receive_detections
|
|
||||||
def hide(frame):
|
|
||||||
phase = None if host._motion is None else host._motion.phase
|
|
||||||
if frame.view == "front" and ((fault == "missing_sweep_tag" and phase == "sweep")
|
|
||||||
or (fault == "missing_zero_tag" and phase == "joint_zero")):
|
|
||||||
frame = replace(frame, detections=tuple(tag for tag in frame.detections if tag.tag_id != 2))
|
|
||||||
return receive(frame)
|
|
||||||
monkeypatch.setattr(host, "receive_detections", hide)
|
|
||||||
if fault == "unstable_feedback":
|
|
||||||
steady_ready = MotionExecution.steady_ready
|
|
||||||
monkeypatch.setattr(MotionExecution, "steady_ready", lambda segment, now:
|
|
||||||
False if segment.command.phase == "joint_zero" and "rh_thumb_ip" in segment.command.zero_joints
|
|
||||||
else steady_ready(segment, now))
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host.execution.session.phase in TERMINAL_PHASES)
|
|
||||||
assert set(host.execution.zero_references) == {"rh_thumb_cmc_yaw"}
|
|
||||||
assert not any(row.get("kind") in {"joint_sample", "joint_zero_sample", "secondary_joint_sample", "pnp_candidate_frame"}
|
|
||||||
and row.get("view") == "front" for row in host.raw_records)
|
|
||||||
assert not host.finalization.started and not host.final_json and not host.final_urdf
|
|
||||||
attempts = [r for r in host.raw_records if r.get("kind") == "diagnostic_joint_zero_attempt"]
|
|
||||||
if fault in {"missing_zero_tag", "unstable_feedback"}:
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert not attempts and not host.execution.diagnostic_zero_attempts
|
|
||||||
expected = ("diagnostic_joint_zero_observations_missing" if fault == "missing_zero_tag"
|
|
||||||
else "diagnostic_joint_zero_feedback_unstable")
|
|
||||||
assert expected in host.reason
|
|
||||||
else:
|
|
||||||
assert len(attempts) == 1
|
|
||||||
assert set(host.execution.diagnostic_zero_attempts) == FRONT_JOINTS
|
|
||||||
checks = [r for r in host.raw_records if r.get("kind") == "scan_unit_complete" and r["task_name"] == "thumb_pitch_ip_front"]
|
|
||||||
if fault == "missing_sweep_tag":
|
|
||||||
assert host.execution.session.phase == Phase.PAUSED
|
|
||||||
assert [r["attempt"] for r in checks] == [1, 2]
|
|
||||||
assert all(not r["passed"] for r in checks)
|
|
||||||
else:
|
|
||||||
assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE
|
|
||||||
assert len(host.execution.completed_units) == 4
|
|
||||||
assert len(checks) == 2 and all(r["passed"] for r in checks)
|
|
||||||
assert all(not r["accepted_pose_quality"]["passed"] for r in checks)
|
|
||||||
raw = [r for r in host.raw_records if r.get("kind") == "diagnostic_observation_frame"
|
|
||||||
and r.get("view") == "front" and r.get("sample_phase") == "sweep"]
|
|
||||||
# Independent tracking can recover, but without a zero its
|
|
||||||
# selected pose is still only a diagnostic observation.
|
|
||||||
assert any(item["selected_pose"] is not None for row in raw for role, item in row["tags"].items()
|
|
||||||
if role in {"thumb_pitch", "thumb_ip"})
|
|
||||||
report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text())
|
|
||||||
assert not report["accuracy_validated"] and not report["publication_allowed"]
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"):
|
|
||||||
reject_diagnostic_capture(host.raw_records)
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def raw_reference():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
name = "rh_thumb_cmc_yaw"
|
|
||||||
spec = profile.motion.joint_zero_references[name]
|
|
||||||
motion = MotionCommand("joint_zero", spec.command, 1., task_key=spec.task_key, zero_joints=(name,))
|
|
||||||
rows = []
|
|
||||||
for stamp in range(1, profile.acquisition.fixed_reference_minimum_frames+1):
|
|
||||||
rows.append({"kind": "diagnostic_observation_frame", "sample_phase": "joint_zero",
|
|
||||||
"task_name": spec.task_key, "view": "top", "session_epoch": 2, "motion_version": 3,
|
|
||||||
"zero_joints": [name], "image_stamp_ns": stamp, "command_unit": profile.command.unit,
|
|
||||||
"command_vector": list(spec.command), "feedback_vector": list(spec.command),
|
|
||||||
"tags": {"thumb_yaw": {"tag_role": "thumb_yaw", "tag_id": 7,
|
|
||||||
"corners_xy": [[0., 0.], [20., 0.], [20., 20.], [0., 20.]],
|
|
||||||
"candidate_diagnostics": {"observation_stamp_ns": stamp}}}})
|
|
||||||
return profile, motion, rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("invalid", ["missing", "duplicate", "session", "motion", "stamp", "binding", "baseline", "feedback"])
|
|
||||||
def test_raw_reference_attempt_requires_current_distinct_complete_images(invalid):
|
|
||||||
profile, motion, rows = raw_reference()
|
|
||||||
rows = deepcopy(rows)
|
|
||||||
if invalid == "missing":
|
|
||||||
rows.pop()
|
|
||||||
elif invalid == "duplicate":
|
|
||||||
rows[-1] = deepcopy(rows[-2])
|
|
||||||
elif invalid in {"session", "motion"}:
|
|
||||||
rows[-1]["session_epoch" if invalid == "session" else "motion_version"] += 1
|
|
||||||
elif invalid == "stamp":
|
|
||||||
rows[-1]["tags"]["thumb_yaw"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1
|
|
||||||
elif invalid == "binding":
|
|
||||||
rows[-1]["tags"]["thumb_yaw"]["tag_id"] = 99
|
|
||||||
elif invalid == "baseline":
|
|
||||||
rows[-1]["command_vector"][1] -= 1
|
|
||||||
else:
|
|
||||||
rows[-1]["feedback_vector"] = None
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_joint_zero_observations_missing"):
|
|
||||||
build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved",
|
|
||||||
session_epoch=2, motion_version=3)
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_attempt_is_not_a_formal_reference_or_reusable_sample():
|
|
||||||
profile, motion, rows = raw_reference()
|
|
||||||
attempt = build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved",
|
|
||||||
session_epoch=2, motion_version=3)
|
|
||||||
execution = SessionExecution(profile)
|
|
||||||
execution._effects = [motion]
|
|
||||||
with pytest.raises(ValueError, match="outside_current_motion"):
|
|
||||||
execution.record_diagnostic_zero_attempt(attempt)
|
|
||||||
execution.diagnostic_zero_attempts[motion.zero_joints[0]] = attempt
|
|
||||||
with pytest.raises(RuntimeError, match="without frozen evidence"):
|
|
||||||
execution.motion_complete()
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"):
|
|
||||||
reject_diagnostic_capture([attempt.as_record()])
|
|
||||||
assert not execution.zero_references
|
|
||||||
|
|
||||||
|
|
||||||
def test_raw_reference_binding_follows_virtual_task_and_view_names():
|
|
||||||
profile, motion, rows = raw_reference()
|
|
||||||
task_name, view_name = "virtual_arrival", "inspection_camera"
|
|
||||||
profile = replace(profile,
|
|
||||||
vision=replace(profile.vision, views=tuple(replace(view, name=view_name)
|
|
||||||
if view.name == "top" else view for view in profile.vision.views)),
|
|
||||||
measurement=replace(profile.measurement, measurements={name:
|
|
||||||
replace(spec, view=view_name) if spec.view == "top" else spec
|
|
||||||
for name, spec in profile.measurement.measurements.items()}),
|
|
||||||
motion=replace(profile.motion, tasks=tuple(replace(task, key=task_name, view=view_name)
|
|
||||||
if task.key == motion.task_key else task for task in reversed(profile.motion.tasks)),
|
|
||||||
joint_zero_references={name: replace(spec, task_key=task_name)
|
|
||||||
if spec.task_key == motion.task_key else spec
|
|
||||||
for name, spec in profile.motion.joint_zero_references.items()}))
|
|
||||||
motion = replace(motion, task_key=task_name)
|
|
||||||
for row in rows:
|
|
||||||
row.update(task_name=task_name, view=view_name)
|
|
||||||
attempt = build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved",
|
|
||||||
session_epoch=2, motion_version=3)
|
|
||||||
assert attempt.task_name == task_name
|
|
||||||
assert attempt.joint_views == (("rh_thumb_cmc_yaw", view_name),)
|
|
||||||
assert {view for view, _ in attempt.source_images} == {view_name}
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_reference_cannot_relabel_an_arbitrary_target_as_declared_zero():
|
|
||||||
profile, motion, rows = raw_reference()
|
|
||||||
target = list(motion.target)
|
|
||||||
target[1] -= 10
|
|
||||||
motion = replace(motion, target=tuple(target))
|
|
||||||
for row in rows:
|
|
||||||
row["command_vector"] = target
|
|
||||||
row["feedback_vector"] = target
|
|
||||||
with pytest.raises(ValueError, match="declared_pose_mismatch"):
|
|
||||||
build_diagnostic_zero_attempt(profile, motion, rows, reason="unresolved",
|
|
||||||
session_epoch=2, motion_version=3)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("action", ["pause", "abort"])
|
|
||||||
def test_late_unresolved_zero_image_cannot_create_diagnostic_attempt(tmp_path, monkeypatch, action):
|
|
||||||
unresolved_front(monkeypatch)
|
|
||||||
host, clock = start_diagnostic(tmp_path, monkeypatch)
|
|
||||||
entered, release = Event(), Event()
|
|
||||||
try:
|
|
||||||
run_until(host, clock, lambda: host._motion is not None
|
|
||||||
and "rh_thumb_ip" in host._motion.zero_joints and host._steady_capture_after_ns is not None)
|
|
||||||
consume = host.capture.consume
|
|
||||||
def held(frame, motion, **kwargs):
|
|
||||||
result = consume(frame, motion, **kwargs)
|
|
||||||
entered.set()
|
|
||||||
assert release.wait(5)
|
|
||||||
return result
|
|
||||||
monkeypatch.setattr(host.capture, "consume", held)
|
|
||||||
clock.now += .04
|
|
||||||
host.receive_feedback(host.profile.command.names, host.last_command, host.ports.clock_ns())
|
|
||||||
with ThreadPoolExecutor(1) as pool:
|
|
||||||
future = pool.submit(host.receive_detections, projected_frame(host, "front", host.last_command))
|
|
||||||
assert entered.wait(2)
|
|
||||||
if action == "abort":
|
|
||||||
assert host.abort().success
|
|
||||||
else:
|
|
||||||
host._pause("test_pause_during_raw_zero")
|
|
||||||
before = host.raw_path.read_bytes()
|
|
||||||
release.set()
|
|
||||||
future.result(timeout=5)
|
|
||||||
assert host.raw_path.read_bytes() == before
|
|
||||||
assert not host.execution.diagnostic_zero_attempts
|
|
||||||
assert set(host.execution.zero_references) == {"rh_thumb_cmc_yaw"}
|
|
||||||
assert not (host.parameters.session_dir / "diagnostic_capture.json").exists()
|
|
||||||
assert_terminal_is_idle(host, clock, monkeypatch)
|
|
||||||
finally:
|
|
||||||
release.set()
|
|
||||||
host.close()
|
|
||||||
@@ -1,230 +0,0 @@
|
|||||||
"""Diagnostic summaries preserve evidence without claiming calibration accuracy."""
|
|
||||||
|
|
||||||
from copy import deepcopy
|
|
||||||
from dataclasses import replace
|
|
||||||
import json
|
|
||||||
from types import SimpleNamespace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_reference import DiagnosticZeroAttempt
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_report import build_diagnostic_report, fixed_reference_summary, pose_step_summary
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_report import task_observation_summary
|
|
||||||
from linkerhand_calibration.runtime.engine import CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase
|
|
||||||
from linkerhand_calibration.runtime.status import normalize_status, render_status_zh
|
|
||||||
|
|
||||||
|
|
||||||
def pose_row(stamp, angle, *, direction="decreasing", cycle=0, attempt=1, cached=False):
|
|
||||||
return {"sample_phase": "sweep", "view": "front", "task_name": "thumb_pitch_ip_front",
|
|
||||||
"cycle": cycle, "direction": direction, "attempt": attempt, "image_stamp_ns": stamp,
|
|
||||||
"pnp_observation_evidence": {"child": {"tag_role": "thumb_pitch", "tag_id": 1,
|
|
||||||
"pose_source": "cached_fixed_reference" if cached else "current_image",
|
|
||||||
"selected_pose": {"quaternion_xyzw": Rotation.from_euler("z", angle, degrees=True).as_quat().tolist()}}}}
|
|
||||||
|
|
||||||
|
|
||||||
def test_pose_step_report_deduplicates_tag_images_and_keeps_actual_jump():
|
|
||||||
rows = [pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 92)]
|
|
||||||
original = deepcopy(rows)
|
|
||||||
report = pose_step_summary(rows + deepcopy(rows))
|
|
||||||
tag = report["front:thumb_pitch"]
|
|
||||||
assert tag["accepted_image_count"] == 2
|
|
||||||
assert tag["adjacent_pair_count"] == 1
|
|
||||||
assert tag["maximum_rotation_step_deg"] == pytest.approx(92)
|
|
||||||
assert tag["largest_steps"][0]["interval_ms"] == pytest.approx(33)
|
|
||||||
assert tag["largest_steps"][0]["to_image"] == "front:1033000000"
|
|
||||||
assert rows == original
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("change", [
|
|
||||||
{"direction": "increasing"}, {"cycle": 1}, {"attempt": 2}, {"cached": True},
|
|
||||||
])
|
|
||||||
def test_different_units_or_cached_poses_cannot_create_false_adjacent_jumps(change):
|
|
||||||
report = pose_step_summary([pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 92, **change)])
|
|
||||||
assert report["front:thumb_pitch"]["maximum_rotation_step_deg"] is None
|
|
||||||
assert not report["front:thumb_pitch"]["largest_steps"]
|
|
||||||
|
|
||||||
|
|
||||||
def test_long_gap_is_not_reported_as_a_single_frame_jump():
|
|
||||||
report = pose_step_summary([pose_row(1_000_000_000, 0), pose_row(6_000_000_000, 92)])
|
|
||||||
assert report["front:thumb_pitch"]["maximum_rotation_step_deg"] is None
|
|
||||||
|
|
||||||
|
|
||||||
def test_fixed_reference_reports_image_error_without_claiming_measured_zero_motion():
|
|
||||||
rows = [pose_row(1_000_000_000, 0), pose_row(1_033_000_000, 0)]
|
|
||||||
for index, row in enumerate(rows):
|
|
||||||
item = row['pnp_observation_evidence']['child']
|
|
||||||
item.update(tag_role='front_base', tag_id=0, pose_source='verified_fixed_reference',
|
|
||||||
candidate_diagnostics={'observation_stamp_ns': row['image_stamp_ns'],
|
|
||||||
'pose_constraint': 'fixed_reference', 'fixed_reference_verified': True,
|
|
||||||
'fixed_reference_reprojection_error_px': .2 + index*.4})
|
|
||||||
rejected = deepcopy(rows[-1]['pnp_observation_evidence']['child'])
|
|
||||||
rejected.update(selected_pose=None, pose_source='current_image')
|
|
||||||
rejected['candidate_diagnostics'].update(observation_stamp_ns=1_066_000_000,
|
|
||||||
fixed_reference_verified=False, fixed_reference_reprojection_error_px=2.0)
|
|
||||||
events = [{'kind': 'pnp_branch_event', 'view': 'front', 'image_stamp_ns': 1_066_000_000,
|
|
||||||
'pnp_observation_evidence': rejected}]
|
|
||||||
report = fixed_reference_summary(rows + deepcopy(rows) + events)
|
|
||||||
assert report['front:front_base']['image_count'] == 3
|
|
||||||
assert report['front:front_base']['accepted_image_count'] == 2
|
|
||||||
assert report['front:front_base']['maximum_reprojection_error_px'] == 2.0
|
|
||||||
assert pose_step_summary(rows + events) == {}
|
|
||||||
|
|
||||||
|
|
||||||
def test_preset_keeps_profile_immutable_and_rejects_wrong_joint_binding():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
before = deepcopy(profile)
|
|
||||||
plan = resolve_diagnostic_capture(profile, "o6_thumb")
|
|
||||||
assert plan.task_keys == ("thumb_yaw_top", "thumb_pitch_ip_front")
|
|
||||||
assert plan.cycles == (0,)
|
|
||||||
assert profile == before
|
|
||||||
wrong = replace(profile, motion=replace(profile.motion, tasks=(
|
|
||||||
replace(profile.motion.tasks[0], command_index=5), *profile.motion.tasks[1:])))
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_thumb_task_binding_changed"):
|
|
||||||
resolve_diagnostic_capture(wrong, "o6_thumb")
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_terminal_and_cycle_count_are_distinct_from_calibration_pass():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
plan = resolve_diagnostic_capture(profile, "o6_thumb")
|
|
||||||
session = CalibrationSession(profile, diagnostic_capture=plan)
|
|
||||||
session.phase = Phase.SWEEP
|
|
||||||
status = session.status().as_dict()
|
|
||||||
status["diagnostic_capture"] = plan.as_dict()
|
|
||||||
rendered = render_status_zh(normalize_status(status, profile=profile))
|
|
||||||
assert "第 1/1 轮" in rendered
|
|
||||||
assert "诊断扫描" in rendered
|
|
||||||
assert "不发布标定 JSON/URDF" in rendered
|
|
||||||
session.phase = Phase.DIAGNOSTIC_COMPLETE
|
|
||||||
status = normalize_status(session.status().as_dict(), profile=profile)
|
|
||||||
assert status["state"] == "DIAGNOSTIC_COMPLETE"
|
|
||||||
assert status["overall_progress_01"] == 1
|
|
||||||
assert "未进行精度验收" in render_status_zh(status)
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_cannot_authorize_commit_even_if_publication_phase_is_requested(tmp_path):
|
|
||||||
from runtime_host_fixture import coordinator_fixture
|
|
||||||
host, _ = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb")
|
|
||||||
try:
|
|
||||||
host.execution.session.phase = Phase.PUBLISH
|
|
||||||
with pytest.raises(RuntimeError, match="diagnostic_capture_not_for_publication"):
|
|
||||||
host._authorize_commit()
|
|
||||||
assert not host.finalization.started
|
|
||||||
assert not list(tmp_path.rglob("*.urdf"))
|
|
||||||
finally:
|
|
||||||
host.close()
|
|
||||||
|
|
||||||
|
|
||||||
def report_inputs(tmp_path):
|
|
||||||
"""Only report binding is under test; no pose/accuracy evidence is synthesized."""
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
plan = resolve_diagnostic_capture(profile, "o6_thumb")
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
names = ("rh_thumb_cmc_pitch", "rh_thumb_ip")
|
|
||||||
attempt = DiagnosticZeroAttempt(names, "thumb_pitch_ip_front", "motion_geometry_unresolved",
|
|
||||||
1, 11, profile.command.baseline_values, profile.command.baseline_values,
|
|
||||||
(("front", 1000),), tuple((name, "front") for name in names))
|
|
||||||
zero = {"kind": "joint_zero_reference", "joint": "rh_thumb_cmc_yaw"}
|
|
||||||
records = [zero, json.loads(json.dumps(attempt.as_record()))]
|
|
||||||
raw_path = tmp_path / "raw_samples.jsonl"
|
|
||||||
raw_path.write_text("".join(json.dumps(row)+"\n" for row in records))
|
|
||||||
return profile, plan, dict(raw_path=raw_path, records=records,
|
|
||||||
references={"rh_thumb_cmc_yaw": SimpleNamespace(as_record=lambda: zero)},
|
|
||||||
completed_units={(unit.task_key, unit.cycle, unit.direction) for unit in units},
|
|
||||||
expected_units=units, protected_inputs={}, command=profile.command.baseline_values,
|
|
||||||
feedback=profile.command.baseline_values,
|
|
||||||
diagnostic_zero_attempts={name: attempt for name in names})
|
|
||||||
|
|
||||||
|
|
||||||
def test_segmented_task_report_keeps_every_declared_segment(tmp_path):
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_finger_roll_views')
|
|
||||||
units = CalibrationEngine(profile, diagnostic_capture=plan).scan_units()
|
|
||||||
assert len(units) == 4
|
|
||||||
names = next(task.joints for task in profile.motion.tasks if task.key == plan.task_keys[0])
|
|
||||||
attempt = DiagnosticZeroAttempt(names, plan.task_keys[0], 'unresolved', 1, 11,
|
|
||||||
profile.command.baseline_values, profile.command.baseline_values,
|
|
||||||
(('front', 1000),), tuple((name, 'front') for name in names))
|
|
||||||
raw_path = tmp_path/'raw.jsonl'
|
|
||||||
raw_path.write_text(json.dumps(attempt.as_record())+'\n')
|
|
||||||
kwargs = dict(raw_path=raw_path, records=[attempt.as_record()], references={},
|
|
||||||
diagnostic_zero_attempts={name: attempt for name in names},
|
|
||||||
completed_units={unit.identity for unit in units}, expected_units=units,
|
|
||||||
protected_inputs={}, command=profile.command.baseline_values, feedback=profile.command.baseline_values)
|
|
||||||
report = build_diagnostic_report(profile, plan, **kwargs)
|
|
||||||
assert len(report['completed_units']) == 4
|
|
||||||
assert report['publication_allowed'] is False
|
|
||||||
kwargs['completed_units'] = {(unit.task_key, unit.cycle, unit.direction) for unit in units}
|
|
||||||
with pytest.raises(ValueError, match='incomplete_units'):
|
|
||||||
build_diagnostic_report(profile, plan, **kwargs)
|
|
||||||
|
|
||||||
|
|
||||||
def test_task_visibility_does_not_generalize_pinky_to_other_fingers():
|
|
||||||
profile = load_bundled_hand_profile('o30_right_18')
|
|
||||||
plan = resolve_diagnostic_capture(profile, 'o30_all_tasks')
|
|
||||||
row = dict(kind='image_observation_frame', task_name='fingers_mcp_roll_front',
|
|
||||||
view='side', image_stamp_ns=1, sample_phase='sweep', command_vector=[0]*20,
|
|
||||||
feedback_vector=[0]*20, tags={'side_base': {}, 'pinky_pip': {}, 'pinky_dip': {}})
|
|
||||||
report = task_observation_summary(profile, plan, [row, deepcopy(row)])
|
|
||||||
assert len(report) == 17
|
|
||||||
assert report['pinky_dip_side']['image_observation_count'] == 0
|
|
||||||
task = report['fingers_mcp_roll_front']
|
|
||||||
assert task['image_observation_count'] == 1
|
|
||||||
phase = task['views']['side']['sweep']
|
|
||||||
assert phase['pair_frames']['pinky_pip/pinky_dip'] == 1
|
|
||||||
for finger in ('ring', 'middle', 'index'):
|
|
||||||
assert phase['pair_frames'][f'{finger}_pip/{finger}_dip'] == 0
|
|
||||||
assert task['accuracy_validated'] is False
|
|
||||||
changed = {**row, 'tags': {}}
|
|
||||||
with pytest.raises(ValueError, match='duplicate_image_changed'):
|
|
||||||
task_observation_summary(profile, plan, [row, changed])
|
|
||||||
|
|
||||||
|
|
||||||
def test_raw_only_report_has_missing_zeros_without_fabricating_joint_references(tmp_path):
|
|
||||||
profile, plan, kwargs = report_inputs(tmp_path)
|
|
||||||
report = build_diagnostic_report(profile, plan, **kwargs)
|
|
||||||
assert report["missing_joint_zero_references"] == ["rh_thumb_cmc_pitch", "rh_thumb_ip"]
|
|
||||||
assert set(report["joint_zero_references"]) == {"rh_thumb_cmc_yaw"}
|
|
||||||
assert report["joint_zero_status"]["rh_thumb_ip"]["status"] == "unresolved"
|
|
||||||
assert report["joint_zero_status"]["rh_thumb_ip"]["raw_only"] is True
|
|
||||||
assert report["joint_zero_status"]["rh_thumb_cmc_yaw"]["status"] == "confirmed"
|
|
||||||
assert report["joint_sample_counts"]["rh_thumb_ip"] == {}
|
|
||||||
assert report["publication_allowed"] is report["accuracy_validated"] is False
|
|
||||||
|
|
||||||
|
|
||||||
def test_report_separates_hold_images_and_samples_from_sweep_counts(tmp_path):
|
|
||||||
profile, plan, kwargs = report_inputs(tmp_path)
|
|
||||||
for phase in ("steady", "sweep"):
|
|
||||||
identity = {"sample_phase": phase, "task_name": "thumb_yaw_top", "view": "top",
|
|
||||||
"direction": "decreasing", "attempt": 1}
|
|
||||||
kwargs["records"].append({"kind": "joint_sample", "joint": "rh_thumb_cmc_yaw", **identity})
|
|
||||||
kwargs["records"].append({"kind": "diagnostic_observation_frame", **identity})
|
|
||||||
kwargs["raw_path"].write_text("".join(json.dumps(row)+"\n" for row in kwargs["records"]))
|
|
||||||
report = build_diagnostic_report(profile, plan, **kwargs)
|
|
||||||
key = "top:decreasing:attempt=1"
|
|
||||||
assert report["joint_sample_counts"]["rh_thumb_cmc_yaw"][key] == 2 # Historical total retained.
|
|
||||||
assert report["joint_sample_counts_by_phase"]["rh_thumb_cmc_yaw"] == {
|
|
||||||
"steady": {key: 1}, "sweep": {key: 1}}
|
|
||||||
assert report["raw_observation_counts_by_phase"] == {"steady": {key: 1}, "sweep": {key: 1}}
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("fault", ["unrecorded", "overlap", "missing", "binding", "accepted_sample"])
|
|
||||||
def test_report_rejects_missing_or_conflicting_unresolved_zero_evidence(tmp_path, fault):
|
|
||||||
profile, plan, kwargs = report_inputs(tmp_path)
|
|
||||||
attempts = kwargs["diagnostic_zero_attempts"]
|
|
||||||
if fault == "unrecorded":
|
|
||||||
kwargs["records"] = kwargs["records"][:1]
|
|
||||||
elif fault == "overlap":
|
|
||||||
kwargs["references"]["rh_thumb_ip"] = kwargs["references"]["rh_thumb_cmc_yaw"]
|
|
||||||
elif fault == "missing":
|
|
||||||
del attempts["rh_thumb_ip"]
|
|
||||||
elif fault == "binding":
|
|
||||||
attempts["rh_thumb_ip"] = replace(attempts["rh_thumb_ip"], task_name="thumb_yaw_top")
|
|
||||||
kwargs["records"].append(attempts["rh_thumb_ip"].as_record())
|
|
||||||
else:
|
|
||||||
kwargs["records"].append({"kind": "joint_sample", "joint": "rh_thumb_ip"})
|
|
||||||
with pytest.raises(ValueError, match="diagnostic_"):
|
|
||||||
build_diagnostic_report(profile, plan, **kwargs)
|
|
||||||
@@ -1,203 +0,0 @@
|
|||||||
"""Short diagnostic captures reuse the production motion and safety schedule."""
|
|
||||||
|
|
||||||
from dataclasses import replace
|
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference
|
|
||||||
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
||||||
from linkerhand_calibration.runtime.diagnostic_capture import DiagnosticCapturePlan
|
|
||||||
from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine
|
|
||||||
from linkerhand_calibration.runtime.execution import SessionExecution
|
|
||||||
from linkerhand_calibration.runtime.resume import ResumeDecision
|
|
||||||
from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit, observation_streams
|
|
||||||
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase, CalibrationSession
|
|
||||||
from test_joint_zero_runtime import reference_rows
|
|
||||||
|
|
||||||
|
|
||||||
THUMB_TASKS = ("thumb_yaw_top", "thumb_pitch_ip_front")
|
|
||||||
THUMB_JOINTS = {"rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_thumb_ip"}
|
|
||||||
|
|
||||||
|
|
||||||
def thumb_capture():
|
|
||||||
# Reverse the selection to prove that execution follows the Profile order.
|
|
||||||
return DiagnosticCapturePlan(mode="o6_thumb", task_keys=tuple(reversed(THUMB_TASKS)))
|
|
||||||
|
|
||||||
|
|
||||||
def sweep_rows(profile, unit, *, attempt=1, first_stamp=1):
|
|
||||||
task = next(task for task in profile.motion.tasks if task.key == unit.task_key)
|
|
||||||
rows = []
|
|
||||||
for field, joint, spec in observation_streams(profile, task):
|
|
||||||
for index in range(129):
|
|
||||||
rows.append({
|
|
||||||
field: joint, "view": spec.view, "task_name": task.key,
|
|
||||||
"cycle": unit.cycle, "direction": unit.direction, "attempt": attempt,
|
|
||||||
"image_stamp_ns": first_stamp + len(rows), "sample_phase": "sweep",
|
|
||||||
"feedback_u8": unit.start + (unit.end - unit.start) * index / 128,
|
|
||||||
})
|
|
||||||
return rows
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"])
|
|
||||||
def test_formal_schedule_keeps_all_tasks_four_cycles_and_both_directions(layout):
|
|
||||||
profile = load_bundled_hand_profile(layout)
|
|
||||||
all_units = CalibrationEngine(profile).scan_units()
|
|
||||||
units = [unit for unit in all_units if unit.sample_phase == "sweep"]
|
|
||||||
steady = [unit for unit in all_units if unit.sample_phase == "steady"]
|
|
||||||
assert len(steady) == 4 * len(profile.motion.tasks)
|
|
||||||
assert {unit.cycle for unit in steady} == {4, 5}
|
|
||||||
expected = []
|
|
||||||
for task in profile.motion.tasks:
|
|
||||||
forward = "increasing" if task.end_value > task.start_value else "decreasing"
|
|
||||||
reverse = "decreasing" if forward == "increasing" else "increasing"
|
|
||||||
expected.extend((task.key, cycle, direction, start, end)
|
|
||||||
for cycle in (0, 1, 2, 3)
|
|
||||||
for direction, start, end in (
|
|
||||||
(forward, task.start_value, task.end_value),
|
|
||||||
(reverse, task.end_value, task.start_value),
|
|
||||||
))
|
|
||||||
assert [(unit.task_key, unit.cycle, unit.direction, unit.start, unit.end)
|
|
||||||
for unit in units] == expected
|
|
||||||
assert CalibrationSession(profile).status().task.cycle_count == 4
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_selection_preserves_profile_order_range_and_speed():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
diagnostic = CalibrationEngine(profile, diagnostic_capture=thumb_capture()).scan_units()
|
|
||||||
formal = CalibrationEngine(profile).scan_units()
|
|
||||||
assert diagnostic == tuple(unit for unit in formal
|
|
||||||
if unit.task_key in THUMB_TASKS and unit.cycle == 0)
|
|
||||||
assert len(diagnostic) == 4
|
|
||||||
assert [unit.task_key for unit in diagnostic] == [THUMB_TASKS[0]] * 2 + [THUMB_TASKS[1]] * 2
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("failed_attempts", [0, 1, 2])
|
|
||||||
def test_thumb_capture_freezes_only_its_zeros_scans_and_returns_without_fitting(failed_attempts):
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
driver = SessionExecution(profile, diagnostic_capture=thumb_capture())
|
|
||||||
driver.session.device_ready()
|
|
||||||
driver.session.start()
|
|
||||||
command = profile.command.baseline_values
|
|
||||||
rows, trace, references = [], [], {}
|
|
||||||
forbidden = {Phase.FIT, Phase.HOLDOUT_VALIDATE, Phase.BUILD_ARTIFACTS,
|
|
||||||
Phase.VALIDATE_URDF, Phase.PUBLISH, Phase.COMPLETE}
|
|
||||||
for _ in range(100):
|
|
||||||
phase = driver.session.phase
|
|
||||||
assert phase not in forbidden
|
|
||||||
assert driver.session.status().task.cycle_count == 1
|
|
||||||
if phase in {Phase.DIAGNOSTIC_COMPLETE, Phase.PAUSED}:
|
|
||||||
break
|
|
||||||
if phase == Phase.REFERENCE_LOCKING:
|
|
||||||
# Lock all selected tasks' references once, including the front
|
|
||||||
# view needed by the later task; the unused side is not required.
|
|
||||||
assert driver.session.status().task.required_tag_ids_by_view == {
|
|
||||||
"front": (0,), "top": (6,)}
|
|
||||||
driver.session.reference_locked()
|
|
||||||
continue
|
|
||||||
if phase == Phase.EVALUATE:
|
|
||||||
driver.evaluate(rows)
|
|
||||||
continue
|
|
||||||
motion = driver.motion(command)
|
|
||||||
assert motion is not None, phase
|
|
||||||
assert motion.phase not in {"steady_prepare", "steady"}
|
|
||||||
command = motion.target
|
|
||||||
assert command[2:] == profile.command.baseline_values[2:]
|
|
||||||
trace.append(motion)
|
|
||||||
for name in motion.zero_joints:
|
|
||||||
assert name in THUMB_JOINTS
|
|
||||||
assert name not in references
|
|
||||||
reference = build_joint_zero_reference(profile, name, reference_rows(profile, name),
|
|
||||||
session_epoch=1, motion_version=len(trace))
|
|
||||||
references[name] = reference
|
|
||||||
driver.zero_references[name] = reference
|
|
||||||
if motion.phase == "sweep":
|
|
||||||
assert motion.task_key in THUMB_TASKS
|
|
||||||
assert motion.cycle == 0
|
|
||||||
unit = driver.action.scan_unit
|
|
||||||
task = next(task for task in profile.motion.tasks if task.key == unit.task_key)
|
|
||||||
assert set(task.joints) <= references.keys()
|
|
||||||
if len([item for item in trace if item.phase == "sweep"]) > failed_attempts:
|
|
||||||
rows.extend(sweep_rows(profile, unit, attempt=motion.attempt,
|
|
||||||
first_stamp=len(rows) + 1))
|
|
||||||
driver.motion_complete()
|
|
||||||
else:
|
|
||||||
pytest.fail("diagnostic executor failed to reach a terminal state")
|
|
||||||
sweeps = [motion for motion in trace if motion.phase == "sweep"]
|
|
||||||
assert trace[0].phase == "baseline"
|
|
||||||
assert driver.session.status().reference_locked
|
|
||||||
assert references == driver.zero_references
|
|
||||||
assert max(motion.attempt for motion in sweeps) <= 2
|
|
||||||
if failed_attempts:
|
|
||||||
assert sweeps[0].target == sweeps[1].target
|
|
||||||
assert sweeps[0].speed == sweeps[1].speed
|
|
||||||
assert sweeps[1].attempt == 2
|
|
||||||
if failed_attempts == 2:
|
|
||||||
assert driver.session.phase == Phase.PAUSED
|
|
||||||
assert driver.session.status().pause.code == "sweep_quality_failed"
|
|
||||||
assert len(sweeps) == 2
|
|
||||||
return
|
|
||||||
assert set(references) == THUMB_JOINTS
|
|
||||||
assert len(driver.completed_units) == 4
|
|
||||||
assert len(sweeps) == 4 + failed_attempts
|
|
||||||
assert trace[-1].phase == "return"
|
|
||||||
assert command == profile.command.baseline_values
|
|
||||||
status = driver.session.status()
|
|
||||||
assert status.state == status.phase == "DIAGNOSTIC_COMPLETE"
|
|
||||||
assert status.overall_progress_01 == 1.0
|
|
||||||
assert driver.motion(command) is None
|
|
||||||
with pytest.raises(RuntimeError, match="expected FIT"):
|
|
||||||
driver.session.fit_complete()
|
|
||||||
with pytest.raises(RuntimeError, match="expected PUBLISH"):
|
|
||||||
driver.session.published()
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_sweep_quality_does_not_require_formal_steady_samples():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
# Legacy interleaved units require both evidence kinds; diagnostics only
|
|
||||||
# exercise the sweep. Separate production units have their own gates.
|
|
||||||
profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved"))
|
|
||||||
unit = CalibrationEngine(profile).scan_units()[0]
|
|
||||||
rows = sweep_rows(profile, unit)
|
|
||||||
diagnostic = evaluate_capture_unit(profile, unit, 1, rows,
|
|
||||||
first_cycle_spans={}, include_steady=False)
|
|
||||||
formal = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={})
|
|
||||||
assert diagnostic.passed
|
|
||||||
assert not formal.passed
|
|
||||||
assert formal.failures and all(reason.startswith("steady:") for reason in formal.failures)
|
|
||||||
|
|
||||||
|
|
||||||
def test_diagnostic_capture_refuses_resume_reuse_and_restore_before_mutation():
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
driver = SessionExecution(profile, diagnostic_capture=thumb_capture())
|
|
||||||
driver.session.device_ready()
|
|
||||||
with pytest.raises(ValueError, match="diagnostic capture cannot resume"):
|
|
||||||
driver.session.start(resume_requested=True)
|
|
||||||
assert driver.session.phase == Phase.READY
|
|
||||||
decision = ResumeDecision(reuse=True, reason="previous", completed_units=(
|
|
||||||
(THUMB_TASKS[0], 0, "decreasing"),))
|
|
||||||
for call in (
|
|
||||||
lambda: driver.session.resume_checked(decision),
|
|
||||||
lambda: driver.authorize_task_reuse(decision.completed_units),
|
|
||||||
lambda: driver.restore(decision, []),
|
|
||||||
):
|
|
||||||
with pytest.raises(ValueError, match="diagnostic capture cannot"):
|
|
||||||
call()
|
|
||||||
assert not driver.completed_units
|
|
||||||
assert not driver.first_cycle_spans
|
|
||||||
assert not driver.zero_references
|
|
||||||
assert driver.session.phase == Phase.READY
|
|
||||||
|
|
||||||
|
|
||||||
def test_formal_session_cannot_reuse_a_diagnostic_journal():
|
|
||||||
from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION
|
|
||||||
profile = load_bundled_hand_profile("o6_right_8")
|
|
||||||
start = {"profile_id": profile.key.profile_id,
|
|
||||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
|
||||||
"capture_schedule_version": CalibrationEngine(profile).capture_schedule_version,
|
|
||||||
"pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION}
|
|
||||||
formal = CalibrationEngine(profile)
|
|
||||||
assert formal.resume_compatible(start)
|
|
||||||
assert not formal.resume_compatible({**start, "diagnostic_capture": thumb_capture().as_dict()})
|
|
||||||
diagnostic = CalibrationEngine(profile, diagnostic_capture=thumb_capture())
|
|
||||||
assert not diagnostic.resume_compatible(start)
|
|
||||||
assert not diagnostic.resume_compatible({**start, "diagnostic_capture": thumb_capture().as_dict()})
|
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user