离线拟合
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 轻量标定实现与验收边界
|
||||
|
||||
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 不表示执行四轮。
|
||||
- 原始角点、合格 PnP 候选、同帧标签、指令、反馈、时间、方向及参考证据保存。所有实际发送指令另存 `raw_command_sent`，统计包括到位、避让、归位及重试。
|
||||
- 新入口不请求在线父模型、唯一移动标签分支、零位准备往返、辅助消歧或歧义回访。采集只检查同步、覆盖、端点、稳态和检测。
|
||||
- 缺失单元移至队尾补采一次；再次不足记录缺项，继续其他任务，归位后报告失败。硬件停滞、反馈失联、参考移动仍保护停止。
|
||||
- 新断点只接收同协议、同受保护输入、同参考/可核验安装条件。中断的尝试消耗原补采预算；第一单元尚未通过也保留断点。拒绝不兼容数据，不静默重采。不可见安装参考明确拒绝恢复。
|
||||
- 保留原运动路线、避让、四指侧摆分段、训练及验证停点和速度。108 个正式方向单元，训练轮身份 0、1，独立验证轮身份 3。
|
||||
- 移动标签只保存角点、身份、检测质量、相机参数、指令、反馈及时间。在线不运行移动标签 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 拓扑计算四指及非平行拇指。一个掌变换、每个实体标签一个共享安装；不引入各阶段独立六自由度变换。
|
||||
- 五根轴的方向与空间布局初始化掌坐标，子轴补充非平行约束；所有允许零位和双向单调指令映射在原始图像目标中联合求解。首次观测及行程端点不作为物理零位。
|
||||
- 最多 64 个初始化，每个初始化的安装预优化与联合优化合计最多 200 次求值。正式后台进程沿用 600 秒上限；预算耗尽保留失败原因，不自动增加动作。
|
||||
@@ -32,6 +51,18 @@ ros2 run linkerhand_calibration calibrate_hand \
|
||||
--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`，仍需提供当前安装条件下的光学观测。若安装参考不可核验，按拒绝原因处理，不能改哈希绕过。
|
||||
|
||||
2026-09-22 用户明确要求复用已完成的相机标定：可将上述光学观测参数换为
|
||||
@@ -49,12 +80,129 @@ CameraInfo；日志及发布审计记录 `current_optical_verification: not_perf
|
||||
- `raw_joint_holdout.json`：每个保留候选的冻结验证。
|
||||
- `calibration_report.json` / `release_manifest.json`：协议、原始证据、安装参考、光学观测及候选集合身份；最终发布证据。
|
||||
|
||||
## 已验证与尚需实机完成
|
||||
## 历史软件验证（2026-09-22）
|
||||
|
||||
- 全手运动效果测试覆盖 17 个任务、三轮正式往返、原避让和归位；实际 coordinator 虚拟时钟覆盖小指 18 个方向、全部已发送指令，无在线求解。
|
||||
- 有界队尾补采、跨恢复预算、首单元中断发现、协议/参考拒绝、固定参考保护、光学缺失时禁止启动均有测试。
|
||||
- 当时的有界队尾补采、跨恢复预算、首单元中断发现、协议/参考拒绝、固定参考保护、光学缺失时禁止启动均有测试。当前 v2 已改为当前方向立即补一次。
|
||||
- ID5、ID9、ID12 的真实历史双候选角点可作为采集数据；ID10 真实缺测不能变成完整扫描。
|
||||
- 独立 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`。
|
||||
- 该软件产物保留生成时的检验审计；当前更严格的 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 仍为指令→视觉角度，反馈映射只作诊断。
|
||||
测试的分层、选择范围与耗时记录见 [测试说明](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 分阶段训练试验
|
||||
|
||||
新增显式选项 `--training-policy staged_2_to_3`，默认仍为 `fixed`。Tag 数量、编号、尺寸、位置、
|
||||
新增显式选项 `--training-policy staged_2_to_3`，该历史策略以 `fixed` 为对照。Tag 数量、编号、尺寸、位置、
|
||||
相机布局、完整网格、运动速度和验收门限不变。新策略按原任务顺序执行全手轮次 0、全手轮次 1，
|
||||
进行训练检查；必要时仅集中补一次轮次 2，随后冻结整手模型，最后采集独立验证轮次 3。
|
||||
局部零位补采包含同轮几何依赖，分段任务整体补采，共享掌部问题扩大到全手。补采后仍不合格即失败。
|
||||
@@ -47,7 +56,7 @@ JSON→URDF 重建、文件与像素验收、成对发布、失败不更新通
|
||||
|
||||
## O30 自适应训练试验
|
||||
|
||||
默认仍为 `fixed`：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后
|
||||
该历史策略以 `fixed` 为对照：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后
|
||||
检查覆盖、稳态网格、双向重复性和可计算的几何零位质量；证据不足补第三轮，第三轮仍不合格
|
||||
就失败。每个方向的缺样恢复仍最多同速重扫一次，准备缺样仍最多局部恢复一次。
|
||||
速度、双向运动顺序、节点、准备路径及最终精度门限均保留。
|
||||
@@ -840,3 +849,97 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
|
||||
主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接
|
||||
投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 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，覆盖
|
||||
完整依赖任务组延后、训练/验证顺序不变、已完成前缀不变、一次回访预算、真实
|
||||
coordinator 的带视角错误处理、多视角参考变化不被掩盖、失败后归位且不进入拟合、
|
||||
@@ -583,3 +663,120 @@ Schur 协方差与完整逆矩阵一致、冻结模型拒绝无法解释的阶
|
||||
只写入 `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
|
||||
profile_id: O30/right/o30_right_18/v1
|
||||
profile_config: package://linkerhand_calibration/config/profiles/o30_right_18.yaml
|
||||
profile_config_sha256: d782fcea36ba753af18ebde4a48c9f4b74d0a93fd27359a8fbfd70b9d23a49c7
|
||||
profile_config_sha256: 0de1efee3ca95a52d04f289f4a118d2119b81e4a84e247f56f2176ea64130b39
|
||||
model: O30
|
||||
side: right
|
||||
tag_layout: o30_right_18
|
||||
@@ -26,8 +26,8 @@ cameras:
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf
|
||||
source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b
|
||||
camera_extrinsics: config/o30_three_camera_extrinsics_20260922_101452.yaml
|
||||
camera_extrinsics_sha256: d28d3e8771f5bc20d4ca6c9bfd11977f26fefb8478d125a113c013fb7a208120
|
||||
camera_extrinsics: config/o30_three_camera_extrinsics_20260923_181333.yaml
|
||||
camera_extrinsics_sha256: e65a09b31bf10aed2fe0926585d446d209929fe7fc84f55f641b0b08a9ef41aa
|
||||
calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml
|
||||
calibration_config_sha256: aeb7826ac00e7ab4c32b131319b34bc11a849c668f268eec2bb4fa12fb081970
|
||||
tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml
|
||||
|
||||
@@ -70,7 +70,7 @@ command:
|
||||
'17': 17
|
||||
'18': 18
|
||||
'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:
|
||||
views:
|
||||
- name: front
|
||||
@@ -94,22 +94,22 @@ vision:
|
||||
id: 12
|
||||
fixed_reference: false
|
||||
size_m: 0.016
|
||||
link: pinky_metacarpals
|
||||
link: pinky_middle
|
||||
- role: ring_mcp_roll
|
||||
id: 13
|
||||
fixed_reference: false
|
||||
size_m: 0.016
|
||||
link: ring_metacarpals
|
||||
link: ring_middle
|
||||
- role: middle_mcp_roll
|
||||
id: 14
|
||||
fixed_reference: false
|
||||
size_m: 0.016
|
||||
link: middle_metacarpals
|
||||
link: middle_middle
|
||||
- role: index_mcp_roll
|
||||
id: 15
|
||||
fixed_reference: false
|
||||
size_m: 0.016
|
||||
link: index_metacarpals
|
||||
link: index_middle
|
||||
- name: side
|
||||
tags:
|
||||
- role: side_base
|
||||
@@ -168,7 +168,8 @@ vision:
|
||||
id: 17
|
||||
fixed_reference: false
|
||||
size_m: 0.016
|
||||
link: thumb_metacarpals_base2
|
||||
# 顶机位标签实际贴在近端指节，随 CMC 和 MCP 运动，不随 IP 运动。
|
||||
link: thumb_proximal
|
||||
common_frame: calibration_common
|
||||
extrinsic_reference_view: front
|
||||
extrinsics_quality_limits:
|
||||
|
||||
@@ -16,6 +16,8 @@ STAGED = "staged_2_to_3"
|
||||
TASKWISE_STAGED = "taskwise_staged_2_to_3"
|
||||
TASKWISE_TWO_ROUND = "taskwise_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))
|
||||
PREPARATION_POLICIES = STAGED_POLICIES | {TASKWISE_TWO_ROUND}
|
||||
LEGACY_TRAINING = (0, 1, 2)
|
||||
@@ -75,8 +77,8 @@ class CapturePlan:
|
||||
def task(self, key):
|
||||
return next(task for task in self.tasks if task.task_key == key)
|
||||
|
||||
def as_dict(self):
|
||||
version = ("capture_plan_v6_raw_joint" if self.policy == RAW_JOINT else
|
||||
def as_dict(self, *, legacy_raw=False):
|
||||
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_v3" if self.policy == TASKWISE_STAGED else
|
||||
"capture_plan_v2" if self.policy == STAGED else PLAN_VERSION)
|
||||
@@ -108,12 +110,15 @@ class CapturePlan:
|
||||
if self.policy == RAW_JOINT:
|
||||
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
|
||||
result["observation_protocol"] = {
|
||||
"version": "raw_joint_images_v1",
|
||||
"version": LEGACY_RAW_PROTOCOL if legacy_raw else RAW_PROTOCOL,
|
||||
"online_geometry_authorization": False,
|
||||
"maximum_unit_recaptures": 1,
|
||||
"recapture_order": "queue_tail",
|
||||
"recapture_order": "queue_tail" if legacy_raw else "immediate",
|
||||
"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
|
||||
|
||||
@property
|
||||
@@ -168,8 +173,8 @@ def select_task_training(profile, task_key, cycles):
|
||||
def configure_training(profile, policy):
|
||||
if policy not in {FIXED, ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT}:
|
||||
raise ValueError("unknown_training_policy")
|
||||
if policy == RAW_JOINT and (profile.key.profile_id != 'O30/right/o30_right_18/v1' or profile.retained_joints):
|
||||
raise ValueError("raw_joint_capture_requires_o30_full_hand_profile")
|
||||
if policy == RAW_JOINT and profile.key.profile_id != 'O30/right/o30_right_18/v1':
|
||||
raise ValueError("raw_joint_capture_requires_o30_profile")
|
||||
if profile.artifacts.output_schema_version in {2, 3} and (
|
||||
tuple(profile.quality.training_cycles) != LEGACY_TRAINING
|
||||
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
|
||||
|
||||
@@ -17,6 +17,8 @@ class TaskStatus:
|
||||
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
|
||||
default_factory=dict
|
||||
)
|
||||
segment_key: str = ""
|
||||
segment_commands: Mapping[str, tuple[float, float]] = field(default_factory=dict)
|
||||
|
||||
|
||||
@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))
|
||||
if name in bundle.zero_columns:
|
||||
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):
|
||||
weight = np.where(directions[:,channel] == -1, .5, directions[:,channel] == direction)
|
||||
for node, column in enumerate(bundle.curve_columns[name][direction]):
|
||||
if column >= 0:
|
||||
jac[:,column] += weight*basis[:,node]
|
||||
jac[:,column] += weights[:,direction,node]
|
||||
derivatives[bundle.cad_names[name]] = jac
|
||||
return derivatives
|
||||
|
||||
@@ -62,11 +61,12 @@ def physical_intervals(bundle, candidate):
|
||||
for i, name in enumerate(bundle.joints):
|
||||
if name in bundle.zero_columns:
|
||||
zeros[i,bundle.zero_columns[name]] = 1.
|
||||
basis = bundle.curve_basis(name, bundle.knots[name])
|
||||
for columns in bundle.curve_columns[name]:
|
||||
for direction in (0, 1):
|
||||
basis = bundle.curve_weights(name, bundle.knots[name],
|
||||
np.full(len(bundle.knots[name]), direction))
|
||||
for weights in basis:
|
||||
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:
|
||||
row[column] += weight
|
||||
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
|
||||
|
||||
|
||||
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)
|
||||
primary_counts, pixels, forward_pixels, uncertainty = defaultdict(int), [], [], []
|
||||
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())
|
||||
rotations[name].extend(Rotation.from_matrix(a[:,:3,:3].transpose(0,2,1)@b[:,:3,:3]).magnitude().tolist())
|
||||
reports = {}
|
||||
for name in bundle.joints:
|
||||
for name in names_to_report:
|
||||
if primary_counts[name] < bundle.profile.acquisition.minimum_valid_samples:
|
||||
raise ValueError('raw_joint_holdout_measurements_missing:'+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())
|
||||
and max(uncertainty,default=np.inf) <= np.radians(1)
|
||||
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_reprojection_px=max(pixels,default=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):
|
||||
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.
|
||||
result = least_squares(self.residual, initial, x_scale=self.scale,
|
||||
jac=getattr(self, 'solver_jacobian', getattr(self, 'jacobian', '2-point')),
|
||||
**getattr(self, 'solver_options', {}),
|
||||
max_nfev=getattr(self, 'maximum_joint_evaluations', 200), ftol=1e-9, xtol=1e-9, gtol=1e-7)
|
||||
result = least_squares(lambda values: self.residual(unpack(values)), initial[columns],
|
||||
x_scale=self.scale[columns],
|
||||
jac=(lambda values: jacobian(unpack(values))[:, columns]) if callable(jacobian) else jacobian,
|
||||
**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)
|
||||
return result
|
||||
|
||||
|
||||
+38
-26
@@ -217,6 +217,41 @@ def motion_frame_rejection(frame: MotionEvidenceFrame, relations: Sequence[Motio
|
||||
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]):
|
||||
"""Preserve both continuous families without treating continuity as truth."""
|
||||
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]
|
||||
if any(item.frozen != items[0].frozen for item in items):
|
||||
return None, "motion_candidate_family_interrupted"
|
||||
# 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(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
|
||||
families, reason = connect_candidate_families(items)
|
||||
if reason:
|
||||
return None, reason
|
||||
paths[role] = families
|
||||
# Near-coincident IPPE roots represent one physical hypothesis, not two.
|
||||
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 .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.motion_path import task_segment
|
||||
from ...domain.sampling import command_nodes
|
||||
from ...domain.result import JointMapping
|
||||
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.chains = {role: model._chain(model.parent_joint_by_child[link])
|
||||
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):
|
||||
result = []
|
||||
@@ -137,8 +142,24 @@ class UrdfCommandImages(FingerChainImages):
|
||||
mask = columns >= 0
|
||||
values[mask] = parameters[columns[mask]]
|
||||
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
|
||||
|
||||
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):
|
||||
"""Nonnegative increments enforce the declared native SDK direction."""
|
||||
knots = self.knots[name]
|
||||
@@ -260,16 +281,15 @@ class UrdfCommandImages(FingerChainImages):
|
||||
if name in self.zero_columns:
|
||||
jac[output_rows, self.zero_columns[name]] = pixel_velocity
|
||||
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):
|
||||
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]):
|
||||
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
|
||||
|
||||
def _pose(self, index, role, branch):
|
||||
def _pose_candidates(self, index, role):
|
||||
key = index, role
|
||||
if key not in self._pnps:
|
||||
row, tag = self.rows[index], self.tags[role][1]
|
||||
@@ -278,12 +298,53 @@ class UrdfCommandImages(FingerChainImages):
|
||||
candidates = self._pnps[key]
|
||||
if not candidates:
|
||||
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[:3, :3] = Rotation.from_quat(pose.quaternion_xyzw).as_matrix()
|
||||
camera[:3, 3] = pose.translation_xyz_m
|
||||
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):
|
||||
if len(branches) != 6 or any(b not in (0, 1) for b in branches):
|
||||
raise ValueError('urdf_images_seed_invalid')
|
||||
@@ -304,16 +365,13 @@ class UrdfCommandImages(FingerChainImages):
|
||||
spec = self.profile.measurement.measurements[name]
|
||||
channel = self.channels[name]
|
||||
for direction in range(2):
|
||||
indices = [i for i, row in enumerate(self.rows) if row['task_name'] == task.key
|
||||
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])
|
||||
indices = self.motion_indices(name, direction, 0)
|
||||
if len(indices) < 3:
|
||||
raise ValueError('urdf_images_directional_training_missing:' + name)
|
||||
# A common motion-curve guess is only a numerical initializer.
|
||||
# Each candidate retains its own installation seeds below;
|
||||
# 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])
|
||||
axis, phase = _axis(rotations)
|
||||
sign = joint_input_direction(self.profile, self.model, name)
|
||||
@@ -338,13 +396,10 @@ class UrdfCommandImages(FingerChainImages):
|
||||
task = task_for_joint(self.profile, root)
|
||||
spec = self.profile.measurement.measurements[root]
|
||||
role = spec.child_role
|
||||
indices = [i for i, row in enumerate(self.rows) if row['task_name'] == task.key
|
||||
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]])
|
||||
indices = self.motion_indices(root, 0, 0)
|
||||
if len(indices) < 3:
|
||||
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])
|
||||
axis, _ = _axis(rotations)
|
||||
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)
|
||||
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.
|
||||
|
||||
Five evaluations per installation plus the remaining joint evaluations
|
||||
@@ -423,8 +478,9 @@ class UrdfCommandImages(FingerChainImages):
|
||||
"""
|
||||
from scipy.optimize import least_squares
|
||||
evaluations = 0
|
||||
observations = self.observations if observations is None else observations
|
||||
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]
|
||||
def project(values):
|
||||
mount = rigid(values)
|
||||
@@ -453,13 +509,17 @@ class UrdfCommandImages(FingerChainImages):
|
||||
return evaluations
|
||||
|
||||
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
|
||||
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')
|
||||
variance = max(.03**2, float(fit.fun @ fit.fun/(len(fit.fun)-len(fit.x))))
|
||||
return np.linalg.inv(information)*variance*self.scale[:, None]*self.scale[None, :]
|
||||
variance = max(.03**2, float(fit.fun @ fit.fun/(len(fit.fun)-len(active))))
|
||||
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):
|
||||
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_sample_ids: tuple[str, ...] = ()
|
||||
release_basis: str = "feedback_and_command"
|
||||
command_support: dict | None = None
|
||||
|
||||
|
||||
def _interval(value):
|
||||
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."""
|
||||
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
|
||||
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.command_training_cycles not in {LEGACY_TRAINING, LEGACY_TRAINING[:2], (4,)}
|
||||
or evidence.training_cycles not in cycles or not evidence.training_sample_ids
|
||||
or evidence.command_training_cycles not in cycles | {(4,)}
|
||||
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 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,
|
||||
fitted_mimics_output: Mapping[str, LinearMimicFit] | None = None,
|
||||
preserve_source_mimic: bool = False,
|
||||
allow_single_training_cycle: bool = False,
|
||||
) -> StandardUrdfPlan:
|
||||
"""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]:
|
||||
raise ValueError(f"invalid corrected interval: {name}")
|
||||
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:
|
||||
raise ValueError(f"corrected range exceeds source mechanical range: {name}")
|
||||
if name in envelopes:
|
||||
|
||||
@@ -49,13 +49,13 @@ def compile_capture_scope(profile):
|
||||
suffix = "_" + name
|
||||
return value if path.stem.endswith(suffix) else path.stem + suffix + path.suffix
|
||||
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),
|
||||
joint_zero_references=keep(profile.motion.joint_zero_references)),
|
||||
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),
|
||||
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),
|
||||
artifacts=replace(profile.artifacts,
|
||||
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 tuple(data.get("joint_names", ())) != self.command_layout.names
|
||||
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"]:
|
||||
raise ValueError("o30_sdk_device_changed")
|
||||
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
|
||||
|
||||
|
||||
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."""
|
||||
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"]
|
||||
references = [row for row in records if row.get("kind") == "fixed_base_reference_locked"]
|
||||
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
|
||||
directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve()
|
||||
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)
|
||||
def save():
|
||||
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')
|
||||
from ..motion_provenance import validate_source_geometry
|
||||
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['acquisition_reassessment'] = evidence.reassessed_units
|
||||
if evidence.missing_units:
|
||||
raise ValueError('raw_joint_capture_incomplete:see_missing_units')
|
||||
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
|
||||
reference = next(r for r in records if r.get('kind') == 'fixed_base_reference_locked')
|
||||
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'):
|
||||
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 any(header.get(key) != value for key, value in hashes.items())):
|
||||
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:
|
||||
state.update(reason=str(error), publication_allowed=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'))
|
||||
save()
|
||||
raise
|
||||
|
||||
+66
-15
@@ -2,25 +2,83 @@
|
||||
from dataclasses import asdict
|
||||
|
||||
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.plan import build_standard_correction_plan
|
||||
from .serializers.measured_v3 import REPORT_FORMAT, ZERO_CONVENTION
|
||||
from .serializers.unified_v1 import _mapping
|
||||
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):
|
||||
profile = bundle.profile
|
||||
candidate = fit.candidates[fit.representative]
|
||||
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)
|
||||
candidate_digest = evidence_digest([dict(seed=list(c.seed), parameters=c.parameters.tolist(),
|
||||
covariance=c.covariance.tolist()) for c in fit.candidates])
|
||||
for name, mapping in mappings.items():
|
||||
task = task_for_joint(profile, 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')]
|
||||
rows = joint_training_rows(profile, training_rows, 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:
|
||||
@@ -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,
|
||||
physical_zero_source='joint_urdf_image_graph',
|
||||
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,
|
||||
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()},
|
||||
@@ -66,10 +115,12 @@ def prepare_raw_report(bundle, fit, evidence, validation, *, serial_number, prot
|
||||
feedback_by_index=profile.command.feedback_by_index,
|
||||
feedback_name_aliases=dict(profile.command.feedback_name_aliases),
|
||||
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),
|
||||
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,
|
||||
candidate_uncertainty=fit.uncertainty, candidate_count=len(fit.candidates),
|
||||
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
|
||||
records = load_capture(raw_path)
|
||||
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)
|
||||
reference = next((row for row in records if row.get("kind") == "fixed_base_reference_locked"), {})
|
||||
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:
|
||||
output = output.expanduser().resolve()
|
||||
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
|
||||
if profile.quality.training_policy == STAGED:
|
||||
import shutil
|
||||
|
||||
+11
-1
@@ -117,7 +117,8 @@ def from_report(report):
|
||||
if raw_audit is not None:
|
||||
from ....core.domain.capture_plan import 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('validation_parameters_updated') is not False):
|
||||
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')):
|
||||
raise ValueError('raw_joint_report_digest_invalid')
|
||||
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', {})
|
||||
if (reference.get('kind') != 'raw_joint_training_baseline'
|
||||
or reference.get('training_sha256') != raw_audit.get('training_sha256')
|
||||
|
||||
@@ -41,6 +41,7 @@ class CaptureFrame:
|
||||
observed_roles: frozenset[str] = frozenset()
|
||||
timing: CaptureTiming | None = None
|
||||
command_skew_ns: int | None = None
|
||||
detection_quality: dict | None = None
|
||||
|
||||
|
||||
class _FramePoseEvidence:
|
||||
@@ -331,9 +332,53 @@ class ObservationCapture:
|
||||
"camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]",
|
||||
"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,
|
||||
diagnostic=False, allow_recording=True, accepted_joints: frozenset[str] | None = None,
|
||||
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):
|
||||
raise ValueError("fixed reference monitoring cannot acquire motion or installations")
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
if motion is not None:
|
||||
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,
|
||||
"camera_matrix": frame.camera_matrix.tolist(), "input_is_rectified": True,
|
||||
"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)))
|
||||
if diagnostic_raw:
|
||||
# Raw candidate capture has its own record type. It cannot be
|
||||
# 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,
|
||||
"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": 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})
|
||||
events.append(self._observation_record(frame, motion, evidence,
|
||||
allow_recording, raw_joint=False, command_unit=self.profile.command.unit))
|
||||
if movement is not None or motion is None or not motion.recording or not allow_recording:
|
||||
return tuple(events), movement
|
||||
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]
|
||||
elif 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:
|
||||
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:
|
||||
|
||||
@@ -46,7 +46,7 @@ from ..core.domain.capture_plan import (CapturePlan, ADAPTIVE, STAGED,
|
||||
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:
|
||||
@@ -86,7 +86,7 @@ class CalibrationCoordinator:
|
||||
self.training_worker = TrainingWorker()
|
||||
self.safety = SafetyPolicy(profile)
|
||||
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.trackers = parameters.new_trackers(self.required_observation_views)
|
||||
self.capture = ObservationCapture(profile, reference_lock=self.reference_lock,
|
||||
@@ -119,6 +119,8 @@ class CalibrationCoordinator:
|
||||
self._observation_epoch = 0
|
||||
self._pause_effects_applied = False
|
||||
self.segment = None
|
||||
self._direction_progress = None
|
||||
self._raw_capture_closed = False
|
||||
self._settled_pose = None
|
||||
self._segment_number = 0
|
||||
self._motion = None
|
||||
@@ -291,6 +293,8 @@ class CalibrationCoordinator:
|
||||
with self.step_data_lock:
|
||||
if self.state in {"FAILED", "ABORTED"}:
|
||||
return ServiceResult(False, self.reason)
|
||||
if self.state == "CAPTURE_COMPLETE":
|
||||
return ServiceResult(False, "raw_capture_already_complete")
|
||||
if self.state == "DIAGNOSTIC_COMPLETE":
|
||||
return ServiceResult(False, "diagnostic_capture_already_complete")
|
||||
if self.state == "PASSED":
|
||||
@@ -302,6 +306,8 @@ class CalibrationCoordinator:
|
||||
|
||||
def receive_feedback(self, names, positions, stamp_ns: int, *, channel_stamps_ns=()) -> StateSample | None:
|
||||
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)
|
||||
|
||||
def receive_camera_info(self, view: str, message: CameraModelInput) -> None:
|
||||
@@ -433,12 +439,15 @@ class CalibrationCoordinator:
|
||||
frame = CaptureFrame(view, stamp, matrix, corners,
|
||||
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,
|
||||
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
|
||||
if motion is not None and motion.phase in {"steady", "joint_zero"} and (steady_after is None or stamp < steady_after):
|
||||
record_motion = None
|
||||
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
|
||||
# capture objects; pause/abort invalidates the epoch; movement changes
|
||||
# the motion identity. None of their late results may enter the session.
|
||||
@@ -497,6 +506,7 @@ class CalibrationCoordinator:
|
||||
return
|
||||
sample_rows = [row for row in rows if row.get("kind") != "pnp_branch_event"
|
||||
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
|
||||
and motion.phase == "steady" and motion.minimum_hold_seconds > 0
|
||||
and row.get("kind") == "diagnostic_observation_frame")
|
||||
@@ -633,6 +643,26 @@ class CalibrationCoordinator:
|
||||
self.training_worker.close()
|
||||
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):
|
||||
if self.state in _TERMINAL_STATES:
|
||||
return
|
||||
@@ -667,33 +697,27 @@ class CalibrationCoordinator:
|
||||
motion_id=self.segment.identity if self.segment else "",
|
||||
motion_goals=self.segment.goals if self.segment else ()))
|
||||
if not decision.safe:
|
||||
if (decision.code == 'mechanical_stall' and self.stall_retry is not None
|
||||
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}")
|
||||
self._handle_motion_failure(decision, now)
|
||||
return
|
||||
if self.stall_retry is not None and self.stall_retry.waiting:
|
||||
elapsed = self.stall_retry.resume(now)
|
||||
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
|
||||
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_rows.clear()
|
||||
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))
|
||||
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:
|
||||
reason = self.visual_motion.check(self.segment, now)
|
||||
if reason:
|
||||
@@ -712,6 +736,7 @@ class CalibrationCoordinator:
|
||||
self.capture_index.extend(interrupted)
|
||||
self.execution.completed_units.update(self._resume_checkpoint.completed_units)
|
||||
self._resumed_count = len(self._resume_checkpoint.completed_units)
|
||||
self._resume_message = '原始证据与补采预算已恢复，继续剩余方向'
|
||||
self._raw_resume_import = None
|
||||
return
|
||||
if self._passed_reference_import is not None:
|
||||
@@ -755,6 +780,12 @@ class CalibrationCoordinator:
|
||||
self._verify_resume()
|
||||
return
|
||||
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
|
||||
training_decision = None
|
||||
if self.execution.training_check_due():
|
||||
@@ -902,11 +933,21 @@ class CalibrationCoordinator:
|
||||
if self.visual_motion is not None:
|
||||
self.visual_motion.begin(motion, current, now=now, version=self._segment_number)
|
||||
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,
|
||||
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)
|
||||
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)
|
||||
if self.visual_motion is not None:
|
||||
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()
|
||||
from .scan_quality import observation_streams
|
||||
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
|
||||
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)
|
||||
rows = self._steady_rows
|
||||
if self.raw_joint:
|
||||
from .raw_quality import coverage_records
|
||||
rows = tuple(coverage_records(self.profile, session.current_unit,
|
||||
motion.attempt, self._steady_rows))
|
||||
steady_complete = all(len({r['image_stamp_ns'] for r in rows
|
||||
if r.get(field) == joint and r['view'] == spec.view})
|
||||
>= 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)
|
||||
# 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 = all(len({r["image_stamp_ns"] for r in 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)
|
||||
steady_complete = steady_complete and self.segment.minimum_hold_complete(now)
|
||||
if self.chain_capture is not None:
|
||||
try:
|
||||
@@ -1047,6 +1079,9 @@ class CalibrationCoordinator:
|
||||
return
|
||||
self._pause("joint_zero_unavailable:"+unavailable)
|
||||
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 motion.phase in {'observer_preflight', 'observer_probe'}:
|
||||
self._advance_observer_preparation(motion, now)
|
||||
@@ -1075,6 +1110,8 @@ class CalibrationCoordinator:
|
||||
if phase == Phase.REFERENCE_POSES and session.phase != phase:
|
||||
self._save_reference()
|
||||
self.segment, self._motion = None, None
|
||||
if session.phase == Phase.CAPTURE_COMPLETE:
|
||||
self._finish_raw_capture()
|
||||
if session.phase == Phase.DIAGNOSTIC_COMPLETE:
|
||||
self._finish_diagnostic_capture()
|
||||
|
||||
@@ -1092,6 +1129,27 @@ class CalibrationCoordinator:
|
||||
self._steady_capture_after_ns = None
|
||||
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):
|
||||
"""Save diagnostics under the state lock before exposing completion."""
|
||||
from .diagnostic_report import build_diagnostic_report
|
||||
@@ -1729,11 +1787,11 @@ class CalibrationCoordinator:
|
||||
return self.state_history[-1]
|
||||
|
||||
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)
|
||||
|
||||
def _write_position(self, values):
|
||||
if not self.parameters.commands_enabled:
|
||||
if not self.parameters.commands_enabled or self._raw_capture_closed:
|
||||
return
|
||||
bounded = self.sdk_adapter.validate_command(values)
|
||||
self.ports.publish_position(bounded)
|
||||
@@ -1755,7 +1813,11 @@ class CalibrationCoordinator:
|
||||
command_vector=list(bounded), direction_by_index=list(self._command_directions),
|
||||
phase=motion.phase if motion else self.execution.session.phase.value,
|
||||
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):
|
||||
indices = self.profile.command.command_index_by_joint.values()
|
||||
@@ -2010,13 +2072,17 @@ class CalibrationCoordinator:
|
||||
try:
|
||||
if self._resume_checkpoint is None:
|
||||
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
|
||||
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',
|
||||
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:
|
||||
self._pause('raw_resume_rejected:' + str(error))
|
||||
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"
|
||||
TASKWISE_STAGED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v14_taskwise_staged_training"
|
||||
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,
|
||||
STAGED_CAPTURE_SCHEDULE_VERSION, TASKWISE_STAGED_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")
|
||||
== ACQUISITION_POLICY_VERSION
|
||||
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"
|
||||
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)
|
||||
|
||||
@@ -71,8 +71,11 @@ class MotionCommand:
|
||||
|
||||
class MotionExecution:
|
||||
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)
|
||||
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
|
||||
if profile.vision_motion and visual_observer is None:
|
||||
raise ValueError("visual motion requires its independent image observer")
|
||||
@@ -204,7 +207,7 @@ class MotionExecution:
|
||||
|
||||
@property
|
||||
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]
|
||||
+ math.copysign(self.required_feedback_travel[i], self.deltas[i]),
|
||||
self.resolution, self.resolution)
|
||||
@@ -231,6 +234,10 @@ class MotionExecution:
|
||||
|
||||
def feedback_motion_complete(self):
|
||||
"""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:
|
||||
return self.visual.motion_complete()
|
||||
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
|
||||
or not self.feedback_motion_complete()):
|
||||
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"}:
|
||||
# Data sufficiency is evaluated after the direction; never wait
|
||||
# indefinitely for a Tag. Missing physical motion is instead
|
||||
|
||||
@@ -17,10 +17,15 @@ def visible_roles(row: Mapping, tags: Mapping) -> set[str]:
|
||||
return visible
|
||||
for role, item in observations.items():
|
||||
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)
|
||||
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("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
|
||||
try:
|
||||
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 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.geometry.frozen_evidence import freeze_evidence
|
||||
from .engine import CalibrationEngine
|
||||
@@ -16,15 +16,21 @@ class RawCalibrationEvidence:
|
||||
completed_units: tuple
|
||||
sha256: str
|
||||
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."""
|
||||
if not raw_joint_capture(profile):
|
||||
raise ValueError('raw_capture_protocol_required')
|
||||
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')
|
||||
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()}
|
||||
completions, groups, identities = {}, defaultdict(list), set()
|
||||
latest_attempts = {}
|
||||
@@ -48,6 +54,8 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
||||
raise ValueError('raw_capture_completion_order_invalid')
|
||||
completions[unit] = row
|
||||
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')
|
||||
if (identity in identities or type(identity[1]) is not int or identity[1] <= 0
|
||||
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:
|
||||
raise ValueError('raw_capture_image_contract_changed')
|
||||
groups[unit, attempt].append(row)
|
||||
accepted, selected, failures, spans = [], [], {}, {}
|
||||
accepted, selected, failures, spans, reassessed = [], [], {}, {}, []
|
||||
for identity, unit in units.items():
|
||||
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]):
|
||||
failures[str(identity)] = ('scan_incomplete',)
|
||||
continue
|
||||
@@ -68,6 +77,11 @@ def partition_raw_capture(profile, records, *, require_complete=True):
|
||||
if not quality.passed:
|
||||
failures[str(identity)] = quality.failures
|
||||
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)
|
||||
selected.extend(rows)
|
||||
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))
|
||||
validation = tuple(r for r in frozen if r['cycle'] == profile.quality.holdout_cycle)
|
||||
return RawCalibrationEvidence(training, validation, tuple(accepted), evidence_digest({
|
||||
'plan': CapturePlan.from_profile(profile).sha256, 'rows': frozen}),
|
||||
tuple(dict(unit=key, failures=list(value)) for key, value in failures.items()))
|
||||
'plan': evidence_digest(plan), 'rows': frozen}),
|
||||
tuple(dict(unit=key, failures=list(value)) for key, value in failures.items()), plan, tuple(reassessed))
|
||||
|
||||
|
||||
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):
|
||||
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,
|
||||
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 not engine.resume_compatible(headers[0])):
|
||||
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)
|
||||
checkpoint = cls(headers[0], references[0], rows, frozenset(evidence.completed_units))
|
||||
# Validate order and retry budgets before any live motor command.
|
||||
checkpoint.restore(CalibrationSession(profile))
|
||||
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,
|
||||
'acquisition_policy_version': current['acquisition_policy_version']}
|
||||
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))
|
||||
old = self.fixed_reference.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
|
||||
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):
|
||||
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)
|
||||
drift = np.linalg.norm(np.asarray(old[role]['corners_xy'])
|
||||
-np.asarray(new[role]['corners_xy']), axis=-1)
|
||||
feedback = np.abs(np.asarray(old[role]['feedback'])-np.asarray(new[role]['feedback']))
|
||||
if (not np.all(np.isfinite(drift)) or np.max(drift)
|
||||
> profile.acquisition.fixed_reference_maximum_drift_px
|
||||
or not np.all(np.isfinite(feedback)) or np.max(feedback) > 10):
|
||||
raise ValueError('raw_resume_installation_changed:' + role)
|
||||
if not np.all(np.isfinite(drift)) or not np.all(np.isfinite(feedback)):
|
||||
raise ValueError('raw_resume_installation_invalid:' + role)
|
||||
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)
|
||||
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):
|
||||
"""Replay journal transitions, including failed and interrupted attempts."""
|
||||
@@ -138,4 +168,5 @@ class RawCheckpoint:
|
||||
|
||||
def import_rows(self):
|
||||
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]:
|
||||
"""Map a common runtime reason to a stable code, explanation and action."""
|
||||
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:
|
||||
return ('OBS-HANDOFF-120', f'共享 Tag 的任务交接证据不完整或已发生变化：{reason}',
|
||||
'核对上游回基准的完整原始图像窗口、反馈及时间顺序；交接不能借用验证轮、跨会话旧图或挑选低误差帧。原零位和已通过数据保持冻结。')
|
||||
@@ -170,6 +173,14 @@ def reason_zh(
|
||||
if reason.startswith("mechanical_stall:"):
|
||||
timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason)
|
||||
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:
|
||||
return ('MOTION-STALL-303',
|
||||
f'同一运动连续 3 次失败，每次反馈{duration}未推进；两次重发前均等待 3 秒，已停止。',
|
||||
|
||||
@@ -23,7 +23,7 @@ from ..storage import atomic_write_json
|
||||
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(
|
||||
@@ -154,6 +154,15 @@ def run_online(
|
||||
from dataclasses import replace
|
||||
config = replace(config, profile_config=effective_path,
|
||||
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
|
||||
contract_report = validate_executable_profile(profile, config.source_urdf)
|
||||
atomic_write_json(session/"measurement_contract.json", contract_report)
|
||||
@@ -171,6 +180,10 @@ def run_online(
|
||||
atomic_write_json(session/"resume_selection.json", dict(
|
||||
selected_session=resume.name if resume else None,
|
||||
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
|
||||
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")
|
||||
@@ -259,6 +272,18 @@ def run_online(
|
||||
startup_timeout=startup_timeout,
|
||||
)
|
||||
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 normalize_status(status)["state"] != "DIAGNOSTIC_COMPLETE":
|
||||
_write_failure(config, session, status)
|
||||
@@ -305,7 +330,7 @@ def run_online(
|
||||
deadline = time.monotonic() + 1.0
|
||||
while rclpy.ok() and not aborted.done() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
message = "诊断采集已完成；已取消后续分析" if capture_complete else "已请求中止"
|
||||
message = "采集已完成；已取消后续分析" if capture_complete else "已请求中止"
|
||||
print(f"{message}；本次数据保留在 {session}。", flush=True)
|
||||
return 130
|
||||
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("--commands-disabled", 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",
|
||||
"taskwise_staged_2_to_3", "taskwise_2_plus_1", "raw_joint_2_plus_1"), default=None,
|
||||
help="taskwise_2_plus_1：每任务连续两轮训练加一轮验证、不补轮；其他策略保留原语义，默认沿用配置")
|
||||
@@ -355,11 +384,34 @@ def main(args: list[str] | None = None) -> None:
|
||||
help="当前安装下的多姿态同步标定板角点 JSON；新 O30 协议启动前重新计算光学核验")
|
||||
parser.add_argument("--offline-output", default="")
|
||||
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',
|
||||
choices=("occlusion_aware_witness_v1", "occlusion_aware_witness_v2",
|
||||
"occlusion_aware_trajectory_v3"),
|
||||
help="O30 staged 准备策略；省略取值使用 v3 整段轨迹消歧和有界自动观察；v1/v2 仅保留历史兼容")
|
||||
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 (
|
||||
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")
|
||||
config = load_product_config(
|
||||
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:
|
||||
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)
|
||||
from ..core.domain.capture_plan import CapturePlan
|
||||
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:
|
||||
report["diagnostic_capture"] = {**diagnostic.as_dict(), "publication_allowed": False}
|
||||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
||||
print(f"配置与受保护输入一致：{config.profile_key.profile_id}。此检查不代表 URDF 精度或硬件验收通过。")
|
||||
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_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
|
||||
replay_capture(config, Path(selected.offline_raw),
|
||||
output=Path(selected.offline_output) if selected.offline_output else None,
|
||||
|
||||
@@ -75,7 +75,7 @@ class SafetyPolicy:
|
||||
)
|
||||
health = sample.health
|
||||
if not health.connected:
|
||||
return self._stop("sdk_disconnected", "SDK 通信已失联")
|
||||
return self._stop("sdk_disconnected", "SDK 连接或身份检查未通过", health.diagnostic)
|
||||
if not health.position_mode:
|
||||
return self._stop("wrong_control_mode", "机械手不在位置控制模式")
|
||||
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,
|
||||
records: Sequence[Mapping], *, first_cycle_spans: dict[str, float],
|
||||
include_steady: bool = True,
|
||||
include_steady_endpoints: bool = False,
|
||||
scale_segment_feedback_coverage: bool = False,
|
||||
) -> SweepQuality:
|
||||
"""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 = {}, {}, {}
|
||||
channel = channel_for_joint(profile, name)
|
||||
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
|
||||
if (row.get(field) != name or row.get("view") != spec.view
|
||||
or row.get("task_name") != unit.task_key
|
||||
@@ -82,7 +85,8 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
||||
profile.command.minimum_feedback_values[channel] <= float(feedback)
|
||||
<= profile.command.maximum_feedback_values[channel]):
|
||||
continue
|
||||
feedback_samples[(spec.view, stamp)] = float(feedback)
|
||||
if sample_phase == "sweep":
|
||||
feedback_samples[(spec.view, stamp)] = float(feedback)
|
||||
value = row.get(f"{'command' if command_coverage else 'feedback'}_{native}")
|
||||
if value is None or not math.isfinite(float(value)):
|
||||
continue
|
||||
@@ -95,6 +99,10 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int,
|
||||
command_progress = (float(commanded) - lower) / span
|
||||
if 0 <= command_progress <= 1:
|
||||
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
|
||||
# Physical encoder bounds are distinct from the requested sweep.
|
||||
# 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:
|
||||
metrics[identity] = _command_coverage(metrics[identity], native)
|
||||
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)
|
||||
metrics[identity]["feedback_motion"] = motion_metrics
|
||||
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)
|
||||
|
||||
|
||||
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.
|
||||
|
||||
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.
|
||||
normalized = tuple((value-lower)/span if span > 0 else 0. for value in values)
|
||||
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.,
|
||||
minimum_valid_samples=policy.minimum_valid_samples, minimum_bins=policy.minimum_bins,
|
||||
maximum_unobserved_fraction=policy.maximum_unobserved_fraction,
|
||||
minimum_valid_samples=policy.minimum_valid_samples, minimum_bins=minimum_bins,
|
||||
maximum_unobserved_fraction=maximum_gap_fraction,
|
||||
total_frames=len(values), joint_frame_rate=1., feedback_hz=0., detection_rate=1.)
|
||||
if span < feedback_tolerances(profile.command.unit).minimum_travel:
|
||||
quality = SweepQuality(False, (*quality.failures, "below_motion_resolution"),
|
||||
quality.warnings, quality.metrics)
|
||||
return quality, dict(observed_minimum=lower, observed_maximum=upper, observed_span=span,
|
||||
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"],
|
||||
maximum_bin_gap=quality.metrics["maximum_bin_gap"])
|
||||
|
||||
|
||||
@@ -38,6 +38,7 @@ class CalibrationPhase(str, Enum):
|
||||
PUBLISH = "PUBLISH"
|
||||
COMPLETE = "COMPLETE"
|
||||
DIAGNOSTIC_COMPLETE = "DIAGNOSTIC_COMPLETE"
|
||||
CAPTURE_COMPLETE = "CAPTURE_COMPLETE"
|
||||
PAUSED = "PAUSED"
|
||||
ABORTED = "ABORTED"
|
||||
FAILED = "FAILED"
|
||||
@@ -217,7 +218,6 @@ class CalibrationSession:
|
||||
if raw_joint_capture(self.profile) and not quality.passed:
|
||||
if self.engine.permits_retry("sweep_acquisition", retries):
|
||||
self._retry_by_unit[key] = retries + 1
|
||||
self._units.append(self._units.pop(self._unit_index))
|
||||
self.phase = CalibrationPhase.PREPARE
|
||||
else:
|
||||
self.incomplete_units[key] = {"failures": quality.failures,
|
||||
@@ -350,6 +350,10 @@ class CalibrationSession:
|
||||
|
||||
def return_complete(self) -> None:
|
||||
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:
|
||||
self.fail("raw_capture_incomplete", "原始采集已归位，部分扫描补采后仍不完整",
|
||||
{str(key): value for key, value in self.incomplete_units.items()},
|
||||
@@ -433,9 +437,10 @@ class CalibrationSession:
|
||||
),
|
||||
None,
|
||||
)
|
||||
segment = task_segment(task, unit.segment_key) if task is not None and unit.segment_key else None
|
||||
progress = (
|
||||
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))
|
||||
)
|
||||
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)
|
||||
for view in self.profile.vision.views if view.name in self.required_observation_views}
|
||||
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(
|
||||
self.profile.measurement.cross_view_sources[name]
|
||||
for name in tuple(measurement_names)
|
||||
@@ -489,12 +494,16 @@ class CalibrationSession:
|
||||
),
|
||||
task=TaskStatus(
|
||||
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_count=displayed_cycle_count,
|
||||
direction="" if unit is None else unit.direction,
|
||||
capture_stage=capture_stage,
|
||||
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,
|
||||
**({"pending_unit_recaptures": [list(candidate.identity)
|
||||
|
||||
@@ -18,6 +18,7 @@ if TYPE_CHECKING:
|
||||
_LEGACY_STATES = {
|
||||
Phase.WAIT_DEVICE: "WAIT_DEVICES", Phase.READY: "READY",
|
||||
Phase.COMPLETE: "PASSED", Phase.PAUSED: "PAUSED",
|
||||
Phase.CAPTURE_COMPLETE: "CAPTURE_COMPLETE",
|
||||
Phase.DIAGNOSTIC_COMPLETE: "DIAGNOSTIC_COMPLETE",
|
||||
Phase.ABORTED: "ABORTED", Phase.FAILED: "FAILED",
|
||||
**{phase: "FINALIZING" for phase in (
|
||||
@@ -193,7 +194,7 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot:
|
||||
parameters.serial_number, host.reason, str(parameters.session_dir),
|
||||
dict(host.reference_lock.latest_drift_px),
|
||||
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_direction_changes_by_channel=list(host._actual_direction_changes),
|
||||
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"}
|
||||
_SEGMENTS = {
|
||||
"linked_increasing": "四指正向联动", "linked_decreasing": "四指反向联动",
|
||||
"middle_lower_increasing": "中指向上补段", "middle_lower_decreasing": "中指向下补段",
|
||||
}
|
||||
_PHASES = {
|
||||
"PREPARING_CHECKPOINT": "加载并校验断点证据（未开始运动）",
|
||||
"WAIT_DEVICE": "等待设备", "READY": "设备就绪", "BASELINE": "安全恢复基准",
|
||||
@@ -22,6 +26,7 @@ _PHASES = {
|
||||
"FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "独立验证",
|
||||
"BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF",
|
||||
"PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停",
|
||||
"CAPTURE_COMPLETE": "采集已结束（URDF 尚未验收）",
|
||||
"DIAGNOSTIC_COMPLETE": "诊断采集完成（未进行精度验收）",
|
||||
"ABORTED": "已中止", "FAILED": "失败", "RUNNING": "标定中",
|
||||
"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 "无"
|
||||
phase = str(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 = bool(diagnostic_info)
|
||||
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}",
|
||||
stage_line,
|
||||
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 "无"),
|
||||
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)} 次",
|
||||
]
|
||||
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:
|
||||
label = diagnostic_capture_label(diagnostic_info.get("mode", "o6_thumb"))
|
||||
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',
|
||||
'thumb_ip', 'index_dip', 'middle_dip', 'ring_dip', 'pinky_dip']
|
||||
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))
|
||||
hand = rigid((.11, -.08, .17), (.11, -.04, .72))
|
||||
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